diff --git a/README.md b/README.md index b0fd54b..d92d142 100755 --- a/README.md +++ b/README.md @@ -1,74 +1,95 @@ # RoboBuggy2 -A complete re-write of the old RoboBuggy. +A complete re-write of the old RoboBuggy. This code was run for RD23, RD24 and RD25, on both NAND and Short Circuit. + ---- ## Table of Contents - - Installation - - Quickstart - - Development + - Installation and Initial Setup + - Launching Code + - Infrastructure Documentation + - Code Structure and Documentation --- -## Installation (for Windows) -### Install Softwares: WSL, Ubuntu, Foxglove -- Go to Microsoft Store to install "Ubuntu 20.04.6 LTS". -- Go install Foxglove https://foxglove.dev/. +## Installation and Initial Setup +### Necessary + Recommended Software +- Docker +- Foxglove +- VSCode (recommended) +- Git (recommended) + ### Docker -- You will need [Docker](https://docs.docker.com/get-docker/) installed. +- Installation instructions here: https://docs.docker.com/get-docker/ -### Set up repo in WSL -- To set up ssh key, follow this link: [Connecting to GitHub with SSH](https://docs.github.com/en/authentication/connecting-to-github-with-ssh). -- Note: Ensure that the SSH keys are generated while in the WSL terminal -- In the website above, see these two pages: [Generating a new SSH key and adding it to the ssh-agent](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) and ["Adding a new SSH key to your GitHub account"](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/adding-a-new-ssh-key-to-your-github-account). +### Foxglove +- Installation instructions here: https://foxglove.dev/ -### Clone -- In your terminal type: `$ git clone https://github.com/CMU-Robotics-Club/RoboBuggy2.git`. -- The clone link above is find in github: code -> local -> Clone SSH. -- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/8ea809f7-35f9-4517-b98d-42e2e869d233) +### VSCode +- https://code.visualstudio.com/download +### Git +- https://git-scm.com/downloads -### ROS -- Navigate to `/rb_ws`. This is the catkin workspace where we will be doing all our ROS stuff. -- To build the ROS workspace and source it, run: +### Install Softwares: WSL, Ubuntu (Windows only) +- Go to Microsoft Store to install "Ubuntu 20.04.6 LTS". - catkin_make - source /rb_ws/devel/setup.bash # sets variables so that our package is visible to ROS commands +### Set up repo in WSL +- To set up ssh key, follow this link: [Connecting to GitHub with SSH](https://docs.github.com/en/authentication/connecting-to-github-with-ssh). +- Note: Ensure that the SSH keys are generated while in the WSL terminal +- In the website above, see these two pages: [Generating a new SSH key and adding it to the ssh-agent](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) and ["Adding a new SSH key to your GitHub account"](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/adding-a-new-ssh-key-to-your-github-account). -## Installation (for MacOS) -### Install Softwares: Docker, Foxglove -- Go install Foxglove https://foxglove.dev/. -- You will need [Docker](https://docs.docker.com/get-docker/) installed. ### Apple Silicon Mac Only: -- In Docker Desktop App: go to settings -> general and turn on "Use Rosetta for x86/amd64 emulation on Apple Silicon" +- In Docker Desktop App: go to settings -> general and turn on "Use Rosetta for x86/amd64 emulation on Apple Silicon" ### Set up repository - To set up ssh key, follow this link: [Connecting to GitHub with SSH](https://docs.github.com/en/authentication/connecting-to-github-with-ssh). - Note: Ensure that the SSH keys are generated while in the terminal - In the website above, see these two pages: [Generating a new SSH key and adding it to the ssh-agent](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) and ["Adding a new SSH key to your GitHub account"](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/adding-a-new-ssh-key-to-your-github-account). -### Clone -- In your terminal type: `$ git clone git@github.com:CMU-Robotics-Club/RoboBuggy2.git`. -- The clone link above is find in github: code -> local -> Clone SSH. -- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/8ea809f7-35f9-4517-b98d-42e2e869d233) +### Clone the Repository +This is so you can edit our codebase locally, and sync your changes with the rest of the team through Git. +- In your terminal type: `$ git clone https://github.com/CMU-Robotics-Club/RoboBuggy2.git`. +- The clone link above is the URL or can be found above: code -> local -> Clone HTTPS. -### ROS -- Navigate to `/rb_ws`. This is the catkin workspace where we will be doing all our ROS stuff. -- To build the ROS workspace and source it, run: - catkin_make - source /rb_ws/devel/setup.bash # sets variables so that our package is visible to ROS commands +### Foxglove Visualization +- Foxglove is used to visualize both the simulator and the actual buggy's movements. +- First, you need to import the layout definition into Foxglove. On the top bar, click Layout, then "Import from file". +- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/2aa04083-46b3-42a5-bcc1-99cf7ccdb3d2) +- Go to RoboBuggy2 and choose the file [telematics layout](telematics_layout.json) +- To visualize the simulator, launch the simulator and then launch Foxglove and select "Open Connection" on startup. +- Use this address `ws://localhost:8765` for Foxglove Websocket +- Open Foxglove, choose the third option "start link". +- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/66965d34-502b-4130-976e-1419c0ac5f69) ---- -## Open Docker + + +### X11 Setup (recommended) +- Install the appropriate X11 server on your computer for your respective operating systems (Xming for Windows, XQuartz for Mac, etc.). +- Mac: In XQuartz settings, ensure that the "Allow connections from network clients" under "Security" is checked. +- Windows: Make sure that you're using WSL 2 Ubuntu and NOT command prompt. +- While in a bash shell with the X11 server running, run `xhost +local:docker`. +- Boot up the docker container using the "Alternate Shortcut" above. +- Run `xeyes` while INSIDE the Docker container to test X11 forwarding. If this works, we're good. + + +## Launching Code +### Open Docker - Use `cd` to change the working directory to be `RoboBuggy2` - Then do `./setup_dev.sh` in the main directory (RoboBuggy2) to launch the docker container. Utilize the `--no-gpu`, `--force-gpu`, and `--run-testing` flags as necessary. - Then you can go in the docker container using the `docker exec -it robobuggy2-main-1 bash`. - When you are done, type Ctrl+C and use `$exit` to exit. -## 2D Simulation +### ROS +- Navigate to `/rb_ws`. This is the catkin workspace where we will be doing all our ROS stuff. +- (This should only need to be run the first time you set up the repository) - to build the ROS workspace and source it, run: + catkin_make + source /rb_ws/devel/setup.bash # sets variables so that our package is visible to ROS commands +- To learn ROS on your own, follow the guide on https://wiki.ros.org/ROS/Tutorials. + +### 2D Simulation - Boot up the docker container - Run `roslaunch buggy sim_2d_single.launch` to simulate 1 buggy - See `rb_ws/src/buggy/launch/sim_2d_single.launch` to view all available launch options @@ -81,38 +102,8 @@ A complete re-write of the old RoboBuggy. - To prevent topic name collision, a topic named `t` associated with buggy named `x` have format `x/t`. The names are `SC` and `Nand` in the 2 buggy simulator. In the one buggy simulator, the name can be defined as a launch arg. - See [**Foxglove Visualization**](#foxglove-visualization) for visualizing the simulation. Beware that since topic names are user-defined, you will need to adjust the topic names in each panel. -### Simulator notes -Feedback: -- Longitude + Latitude for Foxglove visualization on map: `/state/pose_navsat` (sensor_msgs/NavSatFix) -- UTM coordinates (assume we're in Zone 17T): `/sim_2d/utm` (geometry_msgs/Pose - position.x = Easting meters , position.y = Northing meters, position.z = heading in degrees from East axis + is CCW) -- INS Simulation: `/nav/odom` (nsg_msgs/Odometry) (**Noise** is implemented to vary ~1cm) -Commands: -- Steering angle: `/buggy/steering` in degrees (std_msgs/Float64) -- Velocity: `/buggy/velocity` in m/s (std_msgs/Float64) - - -## Foxglove Visualization -- Foxglove is used to visualize both the simulator and the actual buggy's movements. -- First, you need to import the layout definition into Foxglove. On the top bar, click Layout, then "Import from file". -- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/2aa04083-46b3-42a5-bcc1-99cf7ccdb3d2) -- Go to RoboBuggy2 and choose the file [telematics layout](telematics_layout.json) -- To visualize the simulator, launch the simulator and then launch Foxglove and select "Open Connection" on startup. -- Use this address `ws://localhost:8765` for Foxglove Websocket -- Open Foxglove, choose the third option "start link". -- ![image](https://github.com/CMU-Robotics-Club/RoboBuggy2/assets/116482510/66965d34-502b-4130-976e-1419c0ac5f69) - -## X11 Setup -Instructions: -- Install the appropriate X11 server on your computer for your respective operating systems (Xming for Windows, XQuartz for Mac, etc.). -- Mac: In XQuartz settings, ensure that the "Allow connections from network clients" under "Security" is checked. -- Windows: Make sure that you're using WSL 2 Ubuntu and NOT command prompt. -- While in a bash shell with the X11 server running, run `xhost +local:docker`. -- Boot up the docker container using the "Alternate Shortcut" above. -- Run `xeyes` while INSIDE the Docker container to test X11 forwarding. If this works, we're good. - ---- -### Connecting to and Launching the RoboBuggy -When launching the buggy: +### Connecting to and Launching the RoboBuggies +When launching Short Circuit: - Connect to the Wi-Fi named ShortCircuit. - In the command line window: SSH to the computer on ShortCircuit and go to folder @@ -123,9 +114,12 @@ Then `$ cd RoboBuggy2` - Go to docker container `$ docker_exec` - Open foxglove and do local connection to “ws://192.168.1.217/8765” -- Roslauch in docker container by `$ roslaunch buggy sc-main.launch` (or `$ roslaunch buggy nand-main.launch` for NAND) +- Roslauch in docker container by `$ roslaunch buggy sc-main.launch` (wait until no longer prints “waiting for covariance to be better”) +When launching NAND: +- Ask software lead (WIP) + When shutting down the buggy: - Stop roslauch `$ ^C (Ctrl+C)` @@ -133,3 +127,18 @@ When shutting down the buggy: `$ exit` - Shutdown the ShortCircuit computer `$ sudo shutdown now` + +## Documentation +### Infrastructure Documentation +Ask Software Lead (WIP) + +### Simulator notes +- Longitude + Latitude for Foxglove visualization on map: `/state/pose_navsat` (sensor_msgs/NavSatFix) +- UTM coordinates (assume we're in Zone 17N): `/sim_2d/utm` (geometry_msgs/Pose - position.x = Easting meters , position.y = Northing meters, position.z = heading in degrees from East axis + is CCW) +- INS Simulation: `/nav/odom` (nsg_msgs/Odometry) (**Noise** is implemented to vary ~1cm) +Commands: +- Steering angle: `/buggy/steering` in degrees (std_msgs/Float64) +- Velocity: `/buggy/velocity` in m/s (std_msgs/Float64) + +### Auton Logic +Ask someone with experience (WIP) diff --git a/rb_ws/src/buggy/launch/nand-system.launch b/rb_ws/src/buggy/launch/nand-system.launch index 973cbf9..d1b3512 100644 --- a/rb_ws/src/buggy/launch/nand-system.launch +++ b/rb_ws/src/buggy/launch/nand-system.launch @@ -6,4 +6,5 @@ + \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sc-system.launch b/rb_ws/src/buggy/launch/sc-system.launch index 219c44b..7a458ef 100644 --- a/rb_ws/src/buggy/launch/sc-system.launch +++ b/rb_ws/src/buggy/launch/sc-system.launch @@ -14,4 +14,5 @@ + \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/watchdog.launch b/rb_ws/src/buggy/launch/watchdog.launch new file mode 100644 index 0000000..a230231 --- /dev/null +++ b/rb_ws/src/buggy/launch/watchdog.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index 800809d..4c87c6b 100755 --- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py @@ -100,13 +100,14 @@ def set_alarm(self, msg): alarm ros topic reader, locked so that only one of the setters runs at once """ with self.lock: + rospy.logdebug(f"Reading alarm of {msg.data}") self.alarm = msg.data def set_steering(self, msg): """ Steering Angle Updater, updates the steering angle locally if updated on ros stopic """ - rospy.loginfo(f"Read steeering angle of: {msg.data}") + rospy.logdebug(f"Read steeering angle of: {msg.data}") # print("Steering angle: " + str(msg.data)) # print("SET STEERING: " + str(msg.data)) with self.lock: @@ -120,7 +121,7 @@ def writer_thread(self): TODO: Does alarm node exist for NAND? """ rospy.loginfo("Starting sending alarm and steering to teensy!") - while True: + while not rospy.is_shutdown(): if self.fresh_steer: with self.lock: self.comms.send_steering(self.steer_angle) @@ -139,7 +140,7 @@ def reader_thread(self): tuple -> (SC, maybe NAND?) Debug Info """ rospy.loginfo("Starting reading odom from teensy!") - while True: + while not rospy.is_shutdown(): packet = self.comms.read_packet() if isinstance(packet, Odometry): @@ -163,7 +164,7 @@ def reader_thread(self): # TODO: Not mock rolled accurately (Needs to be Fact Checked) try: - lat, long = World.utm_to_gps(packet.y, packet.x) + lat, long = World.utm_to_gps(packet.x, packet.y) odom.pose.pose.position.x = long odom.pose.pose.position.y = lat odom.twist.twist.angular.z = packet.heading_rate diff --git a/rb_ws/src/buggy/scripts/visualization/telematics.py b/rb_ws/src/buggy/scripts/visualization/telematics.py index a3e4893..007aeb7 100755 --- a/rb_ws/src/buggy/scripts/visualization/telematics.py +++ b/rb_ws/src/buggy/scripts/visualization/telematics.py @@ -97,8 +97,8 @@ def republish_fixinfo(self, msg, publishers): else: fix_string += "FIX_RTK_FIXED" - fix_string += "\nsbas_used: " + str(msg.sbas_used) - fix_string += "\ndngss_used: " + str(msg.dngss_used) + # fix_string += "\nsbas_used: " + str(msg.sbas_used) + # fix_string += "\ndngss_used: " + str(msg.dngss_used) publishers[0].publish(fix_string) publishers[1].publish(fix_type) diff --git a/rb_ws/src/buggy/scripts/watchdog/watchdog.py b/rb_ws/src/buggy/scripts/watchdog/watchdog.py new file mode 100755 index 0000000..3375861 --- /dev/null +++ b/rb_ws/src/buggy/scripts/watchdog/watchdog.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 + +import argparse + +import rospy + +from std_msgs.msg import Bool, Int8, Float64 + +class Watchdog: + + STEERING_DEVIANCE = 4 #deg + + def __init__(self, self_name) -> None: + self.alarm = 0 #Check this alarm value + """ + 0 - OK + 1 - WARNING + 2 - ERROR + """ + + self.commanded_steering = 0 + self.inAutonSteer = False + + rospy.Subscriber( + self_name + "/buggy/input/steering", Float64, self.set_input_steering + ) + rospy.Subscriber( + self_name + "/buggy/debug/steering_angle", Float64, self.check_stepper_steering + ) + rospy.Subscriber( + self_name + "/buggy/debug/use_auton_steer", Bool, self.set_auton_steer + ) + + self.alarm_publisher = rospy.Publisher(self_name + "/debug/sanity_warning", Int8, queue_size=1) + self.alarm_publish_rate = rospy.Rate(100) #10ms per alarm + + def set_input_steering(self, msg): + rospy.logdebug("Got input steering of: " + str(msg.data)) + self.commanded_steering = msg.data + + def set_auton_steer(self, msg): + if (msg.data and not self.inAutonSteer): + rospy.loginfo("ENTERED AUTON") + if (not msg.data and self.inAutonSteer): + rospy.logwarn("EXITED AUTON") + self.alarm = 0 #No alarm if not in auton + self.inAutonSteer = msg.data + + def check_stepper_steering(self, msg): + stepper_steer = msg.data + rospy.logdebug("Firmware's reported stepper degree: " + str(stepper_steer)) + if (self.alarm < 2): + self.alarm = 0 + if abs(stepper_steer - self.commanded_steering) > Watchdog.STEERING_DEVIANCE: + if self.inAutonSteer: + self.alarm = 2 # ERROR + rospy.logerr("STEPPER DEVIANCE (DEGREES OFF): " + str(abs(stepper_steer - self.commanded_steering))) + else: + rospy.logdebug("(Non Auton) Stepper Deviance of: " + str(abs(stepper_steer - self.commanded_steering))) + + elif abs(stepper_steer - self.commanded_steering) > Watchdog.STEERING_DEVIANCE//2: + if self.inAutonSteer: + self.alarm = max(self.alarm, 1) + rospy.logwarn("STEPPER POSSIBILY DEVIATING (DEGREES OFF): " + str(abs(stepper_steer - self.commanded_steering))) + else: + rospy.logdebug("(Non Auton) Stepper possibly deviating: " + str(abs(stepper_steer - self.commanded_steering))) + + def loop(self): + while not rospy.is_shutdown(): + #publish alarm + ros_alarm = Int8() + ros_alarm.data = self.alarm + self.alarm_publisher.publish(ros_alarm) + + self.alarm_publish_rate.sleep() + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "--self_name", type=str, help="name of ego-buggy", required=True + ) + args, _ = parser.parse_known_args() + self_name = args.self_name + rospy.init_node("watchdog") + rospy.loginfo("INITIALIZED WATCHDOG NODE") + watchdog = Watchdog(self_name=self_name) + watchdog.loop() \ No newline at end of file