diff --git a/go2_auto_dock/go2_auto_dock/go2_dock.py b/go2_auto_dock/go2_auto_dock/go2_dock.py index a1386d7..ba885e2 100644 --- a/go2_auto_dock/go2_auto_dock/go2_dock.py +++ b/go2_auto_dock/go2_auto_dock/go2_dock.py @@ -78,7 +78,7 @@ def trigger_charging_navigation(self): try: # Launch the navigation script - subprocess.Popen(["ros2", "run", "go2_auto_dock", "go2_dock"]) + subprocess.Popen(["ros2", "run", "go2_auto_dock", "go2_nav_to_charger"]) self.charging_navigation_triggered = True self.get_logger().info( "Navigation to charger script launched successfully." diff --git a/go2_auto_dock/go2_auto_dock/go2_nav_to_charger_final.py b/go2_auto_dock/go2_auto_dock/go2_nav_to_charger_final.py index 06f57a8..36b8f69 100644 --- a/go2_auto_dock/go2_auto_dock/go2_nav_to_charger_final.py +++ b/go2_auto_dock/go2_auto_dock/go2_nav_to_charger_final.py @@ -35,8 +35,8 @@ def __init__(self): self.nav_client = ActionClient(self, NavigateToPose, "navigate_to_pose") # Define goal position and orientation (bearing angle) - self.goal_position = {"x": 0.01612, "y": 0.1392, "z": 0.0} - self.goal_orientation = {"x": 0.0, "y": 0.0, "z": 0.9512, "w": 0.3085} + self.goal_position = {"x": -0.9578, "y": -1.952, "z": 0.0} + self.goal_orientation = {"x": 0.0, "y": 0.0, "z": 0.99, "w": -0.067} # Precompute goal yaw (deg) self.goal_yaw_deg = yaw_from_quaternion( diff --git a/go2_auto_dock/setup.py b/go2_auto_dock/setup.py index 01a50a2..227a048 100644 --- a/go2_auto_dock/setup.py +++ b/go2_auto_dock/setup.py @@ -32,7 +32,8 @@ "go2_apriltag_detector = go2_auto_dock.go2_apriltag_detector:main", "go2_tag_follower = go2_auto_dock.go2_tag_follower:main", "go2_tag_charger = go2_auto_dock.go2_tag_charger:main", - "go2_dock = go2_auto_dock.go2_nav_to_charger_final:main", + "go2_battery_monitor = go2_auto_dock.go2_dock:main", + "go2_nav_to_charger = go2_auto_dock.go2_nav_to_charger_final:main", "go2_camera_publisher = go2_auto_dock.go2_camera_with_adjustable_publisher:main", ], }, diff --git a/go2_gazebo_sim/go2_description/CMakeLists.txt b/go2_gazebo_sim/go2_description/CMakeLists.txt index 95b973b..207f272 100644 --- a/go2_gazebo_sim/go2_description/CMakeLists.txt +++ b/go2_gazebo_sim/go2_description/CMakeLists.txt @@ -26,6 +26,16 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY + models + DESTINATION + share/${PROJECT_NAME}) + +install(DIRECTORY + tags + DESTINATION + share/${PROJECT_NAME}) + ament_package() diff --git a/go2_gazebo_sim/go2_description/models/aruco_marker/model.config b/go2_gazebo_sim/go2_description/models/aruco_marker/model.config new file mode 100644 index 0000000..117af98 --- /dev/null +++ b/go2_gazebo_sim/go2_description/models/aruco_marker/model.config @@ -0,0 +1,13 @@ + + + aruco_marker + 1.0 + model.sdf + + OpenMind + hello@openmind.com + + + An ArUco marker for auto docking. + + diff --git a/go2_gazebo_sim/go2_description/models/aruco_marker/model.sdf b/go2_gazebo_sim/go2_description/models/aruco_marker/model.sdf new file mode 100644 index 0000000..9a9f2b9 --- /dev/null +++ b/go2_gazebo_sim/go2_description/models/aruco_marker/model.sdf @@ -0,0 +1,32 @@ + + + + true + + + + + 0.2 0.01 0.2 + + + + + + + 0.2 0.01 0.2 + + + + 1 1 1 1 + 1 1 1 1 + 0 0 0 1 + + + ../../tags/apriltags.png + + + + + + + diff --git a/go2_gazebo_sim/go2_description/tags/apriltags.png b/go2_gazebo_sim/go2_description/tags/apriltags.png new file mode 100644 index 0000000..05f7235 Binary files /dev/null and b/go2_gazebo_sim/go2_description/tags/apriltags.png differ diff --git a/go2_gazebo_sim/go2_description/worlds/home_world.sdf b/go2_gazebo_sim/go2_description/worlds/home_world.sdf new file mode 100644 index 0000000..150f657 --- /dev/null +++ b/go2_gazebo_sim/go2_description/worlds/home_world.sdf @@ -0,0 +1,3280 @@ + + + + 0.001 + 1 + 1000 + + + + + + + ogre2 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + true + + + true + + + + + 0 0 1 + 100 100 + + + + + + + + + + + + + + 0 0 1 + 100 100 + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + 0 0 0 0 -0 0 + false + + + true + 0.5 5 1.25 0 -0 0 + + + + + 11 0.15 2.5 + + + + + + + + + + + + + + 11 0.15 2.5 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 0.5 -4 1.25 0 -0 0 + + + + + 11 0.15 2.5 + + + + + + + + + + + + + + 11 0.15 2.5 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -5 0.5 1.25 0 -0 0 + + + + + 0.15 9 2.5 + + + + + + + + + + + + + + 0.15 9 2.5 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 6 0.5 1.25 0 -0 0 + + + + + 0.15 9 2.5 + + + + + + + + + + + + + + 0.15 9 2.5 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -3.75 0.5 1.25 0 -0 0 + + + + + 2.5 0.12 2.5 + + + + + + + + + + + + + + 2.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -0.929556 0.5 2.3 0 -0 0 + + + + + 1 0.12 0.4 + + + + + + + + + + + + + + 1 0.12 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 0.318901 0.5 1.25 0 -0 0 + + + + + 1.5 0.12 2.5 + + + + + + + + + + + + + + 1.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 1.56303 0.5 2.3 0 -0 0 + + + + + 1 0.12 0.4 + + + + + + + + + + + + + + 1 0.12 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.31 0.5 1.25 0 -0 0 + + + + + 0.5 0.12 2.5 + + + + + + + + + + + + + + 0.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 4.25 0.5 1.25 0 -0 0 + + + + + 3.5 0.12 2.5 + + + + + + + + + + + + + + 3.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -2 0.205752 1.25 0 -0 0 + + + + + 0.12 0.7 2.5 + + + + + + + + + + + + + + 0.12 0.7 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -2 -0.634617 2.3 0 -0 0 + + + + + 0.12 1 0.4 + + + + + + + + + + + + + + 0.12 1 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -2 -2.75 1.25 0 -0 0 + + + + + 0.12 2.5 2.5 + + + + + + + + + + + + + + 0.12 2.5 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.5 3.25 1.25 0 -0 0 + + + + + 0.12 3.5 2.5 + + + + + + + + + + + + + + 0.12 3.5 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.5 0.959848 2.3 0 -0 0 + + + + + 0.12 0.8 0.4 + + + + + + + + + + + + + + 0.12 0.8 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.5 -2.25 1.25 0 -0 0 + + + + + 0.12 3.5 2.5 + + + + + + + + + + + + + + 0.12 3.5 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -3.8127 4.56578 0.4 0 -0 0 + + + 0 0 0 0 0 0 + + + 2.2 0.9 0.45 + + + + + + + + + + + + 0 0 0 0 0 0 + + + 2.2 0.9 0.45 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + 0 0.35 0.35 0 0 0 + + + 2.2 0.2 0.5 + + + + + + + + + + + + 0 0.35 0.35 0 0 0 + + + 2.2 0.2 0.5 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + -1.0 0 0.15 0 0 0 + + + 0.2 0.9 0.3 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + 1.0 0 0.15 0 0 0 + + + 0.2 0.9 0.3 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -3.86253 3.48221 0.22 0 -0 0 + + + + + 1.2 0.6 0.04 + + + + + + + + + + + + + + 1.2 0.6 0.04 + + + + 0.45 0.35 0.25 1 + 0.55 0.45 0.35 1 + + + + -0.5 -0.2 -0.1 0 0 0 + + + 0.029999999999999999 + 0.20000000000000001 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.5 -0.2 -0.1 0 0 0 + + + 0.029999999999999999 + 0.20000000000000001 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + -0.5 0.2 -0.1 0 0 0 + + + 0.029999999999999999 + 0.20000000000000001 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.5 0.2 -0.1 0 0 0 + + + 0.029999999999999999 + 0.20000000000000001 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -3.5 0.784214 0.3 0 -0 0 + + + + + 1.5 0.4 0.6 + + + + + + + + + + + + + + 1.5 0.4 0.6 + + + + 0.2 0.15 0.1 1 + 0.3 0.25 0.2 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -3.5 0.682625 0.85 0 -0 0 + + + + + 1.2 0.08 0.7 + + + + + + + + + + + + + + 1.2 0.05 0.7 + + + + 0.05 0.05 0.05 1 + 0.1 0.1 0.1 1 + + + + 0 0.03 0 0 0 0 + + + 1.25 0.02 0.75 + + + + 0.15 0.15 0.15 1 + 0.2 0.2 0.2 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -1.66029 2.53221 0.35 0 0 -1.57651 + + + + + 0.8 0.8 0.35 + + + + + + + + + + + + + + 0.8 0.8 0.35 + + + + 0.5 0.4 0.3 1 + 0.6 0.5 0.4 1 + + + + 0 0.35 0.3 0 0 0 + + + 0.8 0.15 0.45 + + + + 0.5 0.4 0.3 1 + 0.6 0.5 0.4 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -4.8 2.5 0.9 0 -0 0 + + + + + 0.3 1 1.8 + + + + + + + + + + + + + + 0.3 1 1.8 + + + + 0.4 0.3 0.2 1 + 0.5 0.4 0.3 1 + + + + 0 0 -0.6 0 0 0 + + + 0.28 0.95 0.02 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 0 0 0 0 0 + + + 0.28 0.95 0.02 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 0 0.6 0 0 0 + + + 0.28 0.95 0.02 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -1.2 4.65187 0.3 0 -0 0 + + + + + 0.25 + 0.59999999999999998 + + + + + + + + + + + + + + 0.25 + 0.59999999999999998 + + + + 0.4 0.35 0.3 1 + 0.5 0.45 0.4 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -1.2 4.63964 0.6 0 -0 0 + + + 0 0 0.05 0 0 0 + + + 0.10000000000000001 + 0.10000000000000001 + + + + 0.2 0.2 0.2 1 + 0.3 0.3 0.3 1 + + + + 0 0 0.5 0 0 0 + + + 0.02 + 0.80000000000000004 + + + + 0.6 0.55 0.5 1 + 0.7 0.65 0.6 1 + + + + 0 0 0.95 0 0 0 + + + 0.14999999999999999 + 0.25 + + + + 0.9 0.85 0.7 1 + 0.95 0.9 0.8 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 4 3.5 0 0 -0 0 + + + 0 0 0.2 0 0 0 + + + 2.1 1.6 0.4 + + + + + + + + + + + + 0 0 0.2 0 0 0 + + + 2.1 1.6 0.4 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 0 0.5 0 0 0 + + + 2 1.5 0.2 + + + + + + + + + + + + 0 0 0.5 0 0 0 + + + 2 1.5 0.2 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + + 0 0.85 0.8 0 0 0 + + + 2.1 0.1 1 + + + + + + + + + + + + 0 0.85 0.8 0 0 0 + + + 2.1 0.1 1 + + + + 0.3 0.2 0.12 1 + 0.4 0.3 0.2 1 + + + + -0.45 0.5 0.65 0 0 0 + + + 0.6 0.4 0.12 + + + + 0.85 0.85 0.9 1 + 0.9 0.9 0.95 1 + + + + 0.45 0.5 0.65 0 0 0 + + + 0.6 0.4 0.12 + + + + 0.85 0.85 0.9 1 + 0.9 0.9 0.95 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 5.3 4.42414 0.3 0 -0 0 + + + + + 0.5 0.4 0.6 + + + + + + + + + + + + + + 0.5 0.4 0.6 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 -0.21 0 0 0 0 + + + 0.4 0.02 0.2 + + + + 0.3 0.2 0.1 1 + 0.4 0.3 0.2 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.7 2.89912 0.3 0 -0 0 + + + + + 0.5 0.4 0.6 + + + + + + + + + + + + + + 0.5 0.4 0.6 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 5.3 4.49184 0.6 0 -0 0 + + + 0 0 0.05 0 0 0 + + + 0.080000000000000002 + 0.10000000000000001 + + + + 0.6 0.55 0.5 1 + 0.7 0.65 0.6 1 + + + + 0 0 0.2 0 0 0 + + + 0.12 + 0.20000000000000001 + + + + 0.95 0.9 0.8 1 + 1 0.95 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 4 4.65 1 0 -0 0 + + + + + 1.8 0.6 2 + + + + + + + + + + + + + + 1.8 0.6 2 + + + + 0.4 0.3 0.2 1 + 0.5 0.4 0.3 1 + + + + -0.45 -0.31 0 0 0 0 + + + 0.85 0.02 1.9 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0.45 -0.31 0 0 0 0 + + + 0.85 0.02 1.9 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 5.50802 3.38724 0.45 0 0 -1.54098 + + + + + 1.2 0.5 0.9 + + + + + + + + + + + + + + 1.2 0.5 0.9 + + + + 0.4 0.3 0.2 1 + 0.5 0.4 0.3 1 + + + + 0 -0.26 0.3 0 0 0 + + + 1.1 0.02 0.25 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 -0.26 0 0 0 0 + + + 1.1 0.02 0.25 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 -0.26 -0.3 0 0 0 + + + 1.1 0.02 0.25 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -1.71989 3.44443 0.35 0 0 -0.48062 + + + + + 0.5 0.5 0.35 + + + + + + + + + + + + + + 0.5 0.5 0.08 + + + + 0.6 0.55 0.5 1 + 0.7 0.65 0.6 1 + + + + 0 0.22 0.25 0 0 0 + + + 0.5 0.05 0.5 + + + + 0.6 0.55 0.5 1 + 0.7 0.65 0.6 1 + + + + -0.2 -0.2 -0.175 0 0 0 + + + 0.02 + 0.34999999999999998 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.2 -0.2 -0.175 0 0 0 + + + 0.02 + 0.34999999999999998 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + -0.2 0.2 -0.175 0 0 0 + + + 0.02 + 0.34999999999999998 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.2 0.2 -0.175 0 0 0 + + + 0.02 + 0.34999999999999998 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -4.6 -1.75 0.45 0 -0 0 + + + + + 0.65 4.5 0.9 + + + + + + + + + + + + + + 0.65 4.5 0.9 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + 0 0 0.47 0 0 0 + + + 0.68 4.55 0.04 + + + + 0.3 0.3 0.3 1 + 0.4 0.4 0.4 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -4.6 -2.5 0.92 0 -0 0 + + + + + 0.6 0.6 0.05 + + + + + + + + + + + + + + 0.6 0.6 0.05 + + + + 0.1 0.1 0.1 1 + 0.15 0.15 0.15 1 + + + + -0.15 0.15 0.03 0 0 0 + + + 0.10000000000000001 + 0.02 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + + 0.15 0.15 0.03 0 0 0 + + + 0.080000000000000002 + 0.02 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + + -0.15 -0.15 0.03 0 0 0 + + + 0.080000000000000002 + 0.02 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + + 0.15 -0.15 0.03 0 0 0 + + + 0.10000000000000001 + 0.02 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -4.6 -0.5 0.9 0 -0 0 + + + + + 0.5 0.6 0.2 + + + + + + + + + + + + + + 0.5 0.6 0.2 + + + + 0.7 0.7 0.72 1 + 0.8 0.8 0.82 1 + + + + 0 0.2 0.2 0 0 0 + + + 0.02 + 0.29999999999999999 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + 0 0.1 0.35 1.57 0 0 + + + 0.014999999999999999 + 0.20000000000000001 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -4.7 -1.75 1.7 0 -0 0 + + + + + 0.35 4 0.8 + + + + + + + + + + + + + + 0.35 4 0.8 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + 0.18 -1.5 0 0 0 0 + + + 0.02 0.9 0.75 + + + + 0.8 0.8 0.75 1 + 0.85 0.85 0.8 1 + + + + 0.18 -0.5 0 0 0 0 + + + 0.02 0.9 0.75 + + + + 0.8 0.8 0.75 1 + 0.85 0.85 0.8 1 + + + + 0.18 0.5 0 0 0 0 + + + 0.02 0.9 0.75 + + + + 0.8 0.8 0.75 1 + 0.85 0.85 0.8 1 + + + + 0.18 1.5 0 0 0 0 + + + 0.02 0.9 0.75 + + + + 0.8 0.8 0.75 1 + 0.85 0.85 0.8 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -2.5 -3.5 0.9 0 -0 0 + + + + + 0.8 0.7 1.8 + + + + + + + + + + + + + + 0.8 0.7 1.8 + + + + 0.7 0.7 0.72 1 + 0.8 0.8 0.82 1 + + + + 0.41 -0.25 0.4 0 0 0 + + + 0.02 0.1 0.3 + + + + 0.5 0.5 0.52 1 + 0.6 0.6 0.62 1 + + + + 0.41 -0.25 -0.5 0 0 0 + + + 0.02 0.1 0.25 + + + + 0.5 0.5 0.52 1 + 0.6 0.6 0.62 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -4.6 0.248494 1.05 0 -0 0 + + + + + 0.35 0.5 0.3 + + + + + + + + + + + + + + 0.35 0.5 0.3 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + + 0.18 0 0 0 0 0 + + + 0.02 0.4 0.25 + + + + 0.1 0.1 0.1 1 + 0.15 0.15 0.15 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -2.2 -1.5 0.3 0 -0 0 + + + + + 0.14999999999999999 + 0.59999999999999998 + + + + + + + + + + + + + + 0.14999999999999999 + 0.59999999999999998 + + + + 0.4 0.4 0.42 1 + 0.5 0.5 0.52 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 5 -3.46048 0.3 0 -0 0 + + + + + 1.7 0.8 0.6 + + + + + + + + + + + + + + 1.7 0.8 0.6 + + + + 0.9 0.9 0.92 1 + 0.95 0.95 0.97 1 + + + + 0 0 0.05 0 0 0 + + + 1.5 0.6 0.5 + + + + 0.85 0.85 0.88 1 + 0.9 0.9 0.93 1 + + + + 0.7 0 0.35 0 0 0 + + + 0.029999999999999999 + 0.14999999999999999 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 5 -3.4766 2 0 -0 0 + + + + + 1.8 0.02 0.02 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + 0 -0.4 -0.8 0 0 0 + + + 1.7 0.02 1.6 + + + + 0.8 0.85 0.9 1 + 0.85 0.9 0.95 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.80821 -3.49969 0 0 -0 1.57 + + + 0 0 0.2 0 0 0 + + + 0.65 0.4 0.4 + + + + + + + + + + + + 0 0 0.2 0 0 0 + + + 0.65 0.4 0.4 + + + + 0.9 0.9 0.92 1 + 0.95 0.95 0.97 1 + + + + 0.15 0 0.35 0 0 0 + + + 0.17999999999999999 + 0.10000000000000001 + + + + 0.88 0.88 0.9 1 + 0.93 0.93 0.95 1 + + + + -0.2 0 0.55 0 0 0 + + + 0.2 0.38 0.35 + + + + + + + + + + + + -0.2 0 0.55 0 0 0 + + + 0.2 0.38 0.35 + + + + 0.9 0.9 0.92 1 + 0.95 0.95 0.97 1 + + + + 0.1 0 0.45 0 0 0 + + + 0.45 0.38 0.05 + + + + 0.88 0.88 0.9 1 + 0.93 0.93 0.95 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.84233 -1.2 0.4 0 -0 0 + + + + + 0.5 0.8 0.8 + + + + + + + + + + + + + + 0.5 0.8 0.8 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + 0 0 0.42 0 0 0 + + + 0.55 0.85 0.04 + + + + 0.7 0.7 0.72 1 + 0.8 0.8 0.82 1 + + + + 0 0 0.45 0 0 0 + + + 0.20000000000000001 + 0.14999999999999999 + + + + 0.85 0.85 0.88 1 + 0.9 0.9 0.93 1 + + + + 0 0.25 0.6 0 0 0 + + + 0.02 + 0.20000000000000001 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.59974 -1.2 1.4 0 -0 0 + + + + + 0.05 0.7 0.9 + + + + 0.4 0.35 0.3 1 + 0.5 0.45 0.4 1 + + + + 0.03 0 0 0 0 0 + + + 0.01 0.6 0.8 + + + + 0.7 0.75 0.8 1 + 0.8 0.85 0.9 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 5.8 -1.5 1 0 -0 0 + + + + + 0.04 0.6 0.04 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + 0.15 0 -0.3 0 0 0 + + + 0.02 0.5 0.7 + + + + 0.3 0.5 0.6 1 + 0.4 0.6 0.7 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.59852 -2.8 0.7 0 -0 0 + + + + + 0.05 0.15 0.1 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + 0.08 0 0 1.57 0 0 + + + 0.059999999999999998 + 0.12 + + + + 0.95 0.95 0.95 1 + 1 1 1 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -1.56216 0.500924 2.3 0 -0 0 + + + + + 1 0.12 0.4 + + + + + + + + + + + + + + 1 0.12 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -1.99415 -1.14358 2.3 0 -0 0 + + + + + 0.12 1 0.4 + + + + + + + + + + + + + + 0.12 1 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -1.52043 -3.40428 0.35 0 -0 1.5412 + + + + + 0.8 0.8 0.35 + + + + + + + + + + + + + + 0.8 0.8 0.35 + + + + 0.5 0.4 0.3 1 + 0.6 0.5 0.4 1 + + + + 0 0.35 0.3 0 0 0 + + + 0.8 0.15 0.45 + + + + 0.5 0.4 0.3 1 + 0.6 0.5 0.4 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 1.29202 -1.66458 0.4 0 -0 0 + + + 0 0 0 0 0 0 + + + 2.2 0.9 0.45 + + + + + + + + + + + + 0 0 0 0 0 0 + + + 2.2 0.9 0.45 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + 0 0.35 0.35 0 0 0 + + + 2.2 0.2 0.5 + + + + + + + + + + + + 0 0.35 0.35 0 0 0 + + + 2.2 0.2 0.5 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + -1.0 0 0.15 0 0 0 + + + 0.2 0.9 0.3 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + 1.0 0 0.15 0 0 0 + + + 0.2 0.9 0.3 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + 2.50165 1.39644 2.3 0 -0 0 + + + + + 0.12 0.8 0.4 + + + + + + + + + + + + + + 0.12 0.8 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + true + -4.30296 0.784659 0.3 0 -0 0 + + + + + 1.5 0.4 0.6 + + + + + + + + + + + + + + 1.5 0.4 0.6 + + + + 0.2 0.15 0.1 1 + 0.3 0.25 0.2 1 + + + + false + + + true + -3.20811 0.514428 1.25 0 -0 0 + + + + + 2.5 0.12 2.5 + + + + + + + + + + + + + + 2.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + false + + + 0 0 10 0 -0 0 + true + 1 + -0.5 0.1 -0.9 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.01 + 0.90000000000000002 + 0.001 + + + 0 + 0 + 0 + + + + -2 2 2.4 0 -0 0 + false + 1 + 0 0 -1 + 1 0.95 0.8 1 + 0.1 0.1 0.1 1 + + 10 + 0.10000000000000001 + 0.5 + 0.01 + + + 0 + 0 + 0 + + + + 3.5 3 2.4 0 -0 0 + false + 1 + 0 0 -1 + 1 0.95 0.8 1 + 0.1 0.1 0.1 1 + + 8 + 0.10000000000000001 + 0.5 + 0.01 + + + 0 + 0 + 0 + + + + -2 -2.5 2.4 0 -0 0 + false + 1 + 0 0 -1 + 1 1 1 1 + 0.2 0.2 0.2 1 + + 6 + 0.10000000000000001 + 0.5 + 0.01 + + + 0 + 0 + 0 + + + + 3.5 -2 2.4 0 -0 0 + false + 1 + 0 0 -1 + 1 1 1 1 + 0.2 0.2 0.2 1 + + 5 + 0.10000000000000001 + 0.5 + 0.01 + + + 0 + 0 + 0 + + + + diff --git a/go2_gazebo_sim/go2_gazebo_sim/config/gait/gait.yaml b/go2_gazebo_sim/go2_gazebo_sim/config/gait/gait.yaml index fcc876a..26763d8 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/config/gait/gait.yaml +++ b/go2_gazebo_sim/go2_gazebo_sim/config/gait/gait.yaml @@ -4,9 +4,9 @@ knee_orientation : ">>" pantograph_leg : false odom_scaler: 0.9 - max_linear_velocity_x : 0.3 - max_linear_velocity_y : 0.25 - max_angular_velocity_z : 0.5 + max_linear_velocity_x : 0.6 + max_linear_velocity_y : 0.6 + max_angular_velocity_z : 1.8 com_x_translation: 0.0 swing_height : 0.04 stance_depth : 0.01 diff --git a/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py b/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py index 021317c..75e2ee7 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py +++ b/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 - import rclpy +from geometry_msgs.msg import Twist +from rcl_interfaces.msg import SetParametersResult from rclpy.node import Node +from rclpy.parameter import Parameter from sensor_msgs.msg import JointState from unitree_go.msg import BmsState, IMUState, LowState, MotorState @@ -11,26 +13,96 @@ class Go2LowStateNode(Node): """ A ROS2 node that publishes mock LowState messages for Unitree Go2 simulation. This provides simulated robot state data including IMU, motor, and battery information. + Battery drain is automatically calculated based on cmd_vel activity. """ + # Expected joint names from Gazebo + EXPECTED_JOINT_NAMES = [ + "rf_hip_joint", + "lf_lower_leg_joint", + "rf_lower_leg_joint", + "lf_upper_leg_joint", + "rh_hip_joint", + "rf_upper_leg_joint", + "rh_upper_leg_joint", + "rh_lower_leg_joint", + "lh_hip_joint", + "lf_hip_joint", + "lh_upper_leg_joint", + "lh_lower_leg_joint", + ] + + # Mapping from Gazebo joint order to Unitree convention: + # [rf_hip, rf_upper, rf_lower, lf_hip, lf_upper, lf_lower, + # rh_hip, rh_upper, rh_lower, lh_hip, lh_upper, lh_lower] + UNITREE_JOINT_INDICES = [0, 5, 2, 9, 3, 1, 4, 6, 7, 8, 10, 11] + def __init__(self): super().__init__("go2_lowstate_node") + # Declare battery simulation parameters + self.declare_parameter("soc", 80.0) + self.declare_parameter("drain_rate", 0.01) + self.declare_parameter("idle_drain_rate", 0.001) + self.declare_parameter("charge_rate", 0.05) + self.declare_parameter("is_charging", False) + self.declare_parameter("velocity_threshold", 0.01) + self.declare_parameter("cmd_vel_timeout", 0.5) + + # Publishers self.lowstate_publisher = self.create_publisher(LowState, "/lowstate", 10) + self.lf_lowstate_publisher = self.create_publisher(LowState, "/lf/lowstate", 10) + # Subscribers self.joint_state_subscriber = self.create_subscription( JointState, "/joint_states", self.joint_state_callback, 10 ) + self.cmd_vel_subscriber = self.create_subscription( + Twist, "/cmd_vel", self.cmd_vel_callback, 10 + ) + # Timer for publishing at 100Hz self.timer = self.create_timer(0.01, self.publish_lowstate) + # State variables self.tick_counter = 0 - self.latest_joint_state = None + self.cmd_vel_moving = False + self.last_cmd_vel_time = self.get_clock().now() + + # Parameter callback for runtime changes + self.add_on_set_parameters_callback(self.parameter_callback) self.get_logger().info("LowState Mock Node initialized") - self.get_logger().info("Publishing to: /lowstate at 100Hz") - self.get_logger().info("Subscribing to: /joint_states") + self.get_logger().info("Publishing to: /lowstate and /lf/lowstate at 100Hz") + self.get_logger().info("Subscribing to: /joint_states, /cmd_vel") + self.get_logger().info( + f"Battery: SoC={self.get_parameter('soc').value}%, " + f"drain_rate={self.get_parameter('drain_rate').value}%/tick" + ) + + def parameter_callback(self, params) -> SetParametersResult: + """ + Callback for handling parameter changes at runtime. + + Parameters: + ----------- + params : list + List of parameter changes + + Returns: + -------- + SetParametersResult + Result indicating success or failure + """ + for param in params: + if param.name == "is_charging": + status = "STARTED" if param.value else "STOPPED" + self.get_logger().info(f"Charging {status}") + elif param.name == "soc": + self.get_logger().info(f"SoC manually set to {param.value:.1f}%") + + return SetParametersResult(successful=True) def joint_state_callback(self, msg: JointState): """ @@ -43,6 +115,22 @@ def joint_state_callback(self, msg: JointState): """ self.latest_joint_state = msg + def cmd_vel_callback(self, msg: Twist): + """ + Callback function for velocity commands to detect robot movement. + + Parameters: + ----------- + msg : Twist + Velocity command message + """ + threshold = self.get_parameter("velocity_threshold").value + linear_speed = (msg.linear.x**2 + msg.linear.y**2) ** 0.5 + angular_speed = abs(msg.angular.z) + + self.cmd_vel_moving = linear_speed > threshold or angular_speed > threshold + self.last_cmd_vel_time = self.get_clock().now() + def get_unitree_joint_data(self): """ Convert Gazebo joint states to Unitree motor order. @@ -50,30 +138,16 @@ def get_unitree_joint_data(self): Returns: -------- tuple - (positions, velocities) in Unitree order, or None if no data available + (positions, velocities) in Unitree order, or (None, None) if no data available """ if self.latest_joint_state is None: return None, None - # Expected joint names from Gazebo in this order: - expected_names = [ - "rf_hip_joint", - "lf_lower_leg_joint", - "rf_lower_leg_joint", - "lf_upper_leg_joint", - "rh_hip_joint", - "rf_upper_leg_joint", - "rh_upper_leg_joint", - "rh_lower_leg_joint", - "lh_hip_joint", - "lf_hip_joint", - "lh_upper_leg_joint", - "lh_lower_leg_joint", - ] - name_to_index = {name: i for i, name in enumerate(self.latest_joint_state.name)} - missing_joints = [name for name in expected_names if name not in name_to_index] + missing_joints = [ + name for name in self.EXPECTED_JOINT_NAMES if name not in name_to_index + ] if missing_joints: self.get_logger().warn(f"Missing joints: {missing_joints}") return None, None @@ -81,7 +155,7 @@ def get_unitree_joint_data(self): gazebo_positions = [] gazebo_velocities = [] - for joint_name in expected_names: + for joint_name in self.EXPECTED_JOINT_NAMES: idx = name_to_index[joint_name] gazebo_positions.append(self.latest_joint_state.position[idx]) gazebo_velocities.append( @@ -90,24 +164,58 @@ def get_unitree_joint_data(self): else 0.0 ) - # Reorder to Unitree convention: [rf_hip, rf_upper, rf_lower, lf_hip, lf_upper, lf_lower, - # rh_hip, rh_upper, rh_lower, lh_hip, lh_upper, lh_lower] - # Mapping from gazebo order to unitree order: - unitree_indices = [0, 5, 2, 9, 3, 1, 4, 6, 7, 8, 10, 11] - - unitree_positions = [gazebo_positions[i] for i in unitree_indices] - unitree_velocities = [gazebo_velocities[i] for i in unitree_indices] + unitree_positions = [gazebo_positions[i] for i in self.UNITREE_JOINT_INDICES] + unitree_velocities = [gazebo_velocities[i] for i in self.UNITREE_JOINT_INDICES] return unitree_positions, unitree_velocities - def create_mock_lowstate(self) -> LowState: + def update_battery_state(self): + """ + Update battery state based on robot activity. + + Returns: + -------- + tuple + (soc, current_ma, is_moving) - current battery state + """ + soc = self.get_parameter("soc").value + is_charging = self.get_parameter("is_charging").value + timeout = self.get_parameter("cmd_vel_timeout").value + + # Check if robot is moving based on cmd_vel timeout + time_since_cmd = ( + self.get_clock().now() - self.last_cmd_vel_time + ).nanoseconds / 1e9 + is_moving = self.cmd_vel_moving and time_since_cmd < timeout + + # Don't drain while charging + if is_charging: + is_moving = False + + # Update SoC based on activity + if is_charging: + soc = min(100.0, soc + self.get_parameter("charge_rate").value) + current_ma = 3000 # Charging current + elif is_moving: + soc = max(0.0, soc - self.get_parameter("drain_rate").value) + current_ma = -5000 # High drain while moving + else: + soc = max(0.0, soc - self.get_parameter("idle_drain_rate").value) + current_ma = -500 # Low idle drain + + # Update the parameter with new SoC value + self.set_parameters([Parameter("soc", value=soc)]) + + return soc, current_ma, is_moving + + def create_lowstate_message(self) -> LowState: """ - Create a mock LowState message based on the provided sample data. + Create a LowState message with current robot state. Returns: -------- LowState - A populated LowState message with mock data + A populated LowState message with current data """ msg = LowState() @@ -128,7 +236,6 @@ def create_mock_lowstate(self) -> LowState: msg.imu_state.temperature = 0 # Motor States (20 motors total, 12 active + 8 inactive) - # Get current joint data from Gazebo unitree_positions, unitree_velocities = self.get_unitree_joint_data() # Use default values if no joint data is available yet @@ -136,42 +243,22 @@ def create_mock_lowstate(self) -> LowState: unitree_positions = [0.0] * 12 unitree_velocities = [0.0] * 12 - # Motor data with realistic temperatures and small torque estimates - motor_data = [] - for i in range(12): - motor_data.append( - { - "mode": 1, - "q": unitree_positions[i], - "dq": unitree_velocities[i], - "ddq": 0.0, - "tau_est": 0.025 + (i % 3) * 0.01, # Small varying torque estimates - "q_raw": 0.0, - "dq_raw": 0.0, - "ddq_raw": 0.0, - "temperature": 26 + (i % 6), # Varying temps 26-31°C - "lost": 0, - "reserve": [0, 588], - } - ) - # Create motor state array with exactly 20 elements motor_states = [] for i in range(20): motor = MotorState() if i < 12: - data = motor_data[i] - motor.mode = data["mode"] - motor.q = data["q"] - motor.dq = data["dq"] - motor.ddq = data["ddq"] - motor.tau_est = data["tau_est"] - motor.q_raw = data["q_raw"] - motor.dq_raw = data["dq_raw"] - motor.ddq_raw = data["ddq_raw"] - motor.temperature = data["temperature"] - motor.lost = data["lost"] - motor.reserve = data["reserve"] + motor.mode = 1 + motor.q = unitree_positions[i] + motor.dq = unitree_velocities[i] + motor.ddq = 0.0 + motor.tau_est = 0.025 + (i % 3) * 0.01 # Small varying torque estimates + motor.q_raw = 0.0 + motor.dq_raw = 0.0 + motor.ddq_raw = 0.0 + motor.temperature = 26 + (i % 6) # Varying temps 26-31°C + motor.lost = 0 + motor.reserve = [0, 588] else: motor.mode = 0 motor.q = 0.0 @@ -189,13 +276,15 @@ def create_mock_lowstate(self) -> LowState: msg.motor_state = motor_states - # BMS State + # Battery State (dynamic based on movement) + soc, current_ma, is_moving = self.update_battery_state() + msg.bms_state = BmsState() msg.bms_state.version_high = 1 msg.bms_state.version_low = 18 msg.bms_state.status = 8 - msg.bms_state.soc = 100 - msg.bms_state.current = -2752 + msg.bms_state.soc = int(soc) + msg.bms_state.current = current_ma msg.bms_state.cycle = 5 msg.bms_state.bq_ntc = [25, 23] msg.bms_state.mcu_ntc = [29, 28] @@ -214,7 +303,7 @@ def create_mock_lowstate(self) -> LowState: 0, 0, 0, - 0, # Remaining cells are 0 + 0, ] # Foot force sensors @@ -228,13 +317,15 @@ def create_mock_lowstate(self) -> LowState: # Wireless remote (all zeros) msg.wireless_remote = [0] * 40 + # Power state (calculated from SoC) + msg.power_v = 24.0 + (soc / 100.0) * 9.6 # ~24V at 0%, ~33.6V at 100% + msg.power_a = current_ma / 1000.0 + # Additional fields msg.bit_flag = 36 msg.adc_reel = 0.0032226562034338713 msg.temperature_ntc1 = 43 msg.temperature_ntc2 = 40 - msg.power_v = 28.67633056640625 - msg.power_a = 1.1279981136322021 msg.fan_frequency = [0, 0, 0, 0] msg.reserve = 0 msg.crc = 1036487475 @@ -245,25 +336,31 @@ def publish_lowstate(self): """ Timer callback to publish LowState messages at regular intervals. """ - msg = self.create_mock_lowstate() + msg = self.create_lowstate_message() self.lowstate_publisher.publish(msg) + self.lf_lowstate_publisher.publish(msg) - # Log every 100 messages (1 second at 100Hz) - if self.tick_counter % 100 == 0: + # Log every 500 messages (5 seconds at 100Hz) + if self.tick_counter % 500 == 0: joint_status = ( "No joint data" if self.latest_joint_state is None else "Joint data OK" ) - self.get_logger().debug( - f"Published LowState - Tick: {msg.tick}, " - f"Battery SOC: {msg.bms_state.soc}%, " - f"IMU Temp: {msg.imu_state.temperature}°C, " - f"Joint Status: {joint_status}" + is_charging = self.get_parameter("is_charging").value + status = ( + "CHARGING" + if is_charging + else ("MOVING" if self.cmd_vel_moving else "IDLE") + ) + self.get_logger().info( + f"[{status}] SoC: {msg.bms_state.soc}%, " + f"Current: {msg.bms_state.current}mA, " + f"Joints: {joint_status}" ) def main(args=None): """ - Main entry point for the lowstate_mock_node. + Main entry point for the go2_lowstate_node. """ rclpy.init(args=args) diff --git a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py index ca67202..c99d41f 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py +++ b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py @@ -7,6 +7,7 @@ DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable, TimerAction, ) from launch.conditions import IfCondition @@ -32,7 +33,25 @@ def generate_launch_description(): gait_config = os.path.join(go2_gazebo_sim, "config/gait/gait.yaml") links_config = os.path.join(go2_gazebo_sim, "config/links/links.yaml") default_model_path = os.path.join(go2_description, "urdf/unitree_go2_robot.xacro") - default_world_path = os.path.join(go2_description, "worlds/maze_world.sdf") + default_world_path = os.path.join(go2_description, "worlds/home_world.sdf") + aruco_model_path = os.path.join(go2_description, "models/aruco_marker/model.sdf") + go2_description_models = os.path.join(go2_description, "models") + + # Ensure GZ_SIM_RESOURCE_PATH includes the models directory + current_gz_resource_path = os.environ.get("GZ_SIM_RESOURCE_PATH", "") + if go2_description_models not in current_gz_resource_path: + if current_gz_resource_path: + new_gz_resource_path = ( + current_gz_resource_path + ":" + go2_description_models + ) + else: + new_gz_resource_path = go2_description_models + else: + new_gz_resource_path = current_gz_resource_path + + set_gz_resource_path = SetEnvironmentVariable( + name="GZ_SIM_RESOURCE_PATH", value=new_gz_resource_path + ) declare_use_sim_time = DeclareLaunchArgument( "use_sim_time", @@ -247,6 +266,27 @@ def generate_launch_description(): }.items(), ) + gazebo_spawn_aruco = Node( + package="ros_gz_sim", + executable="create", + name="spawn_aruco", + output="screen", + arguments=[ + "-name", + "aruco_marker", + "-file", + aruco_model_path, + "-x", + "-1.91", + "-y", + "-1.99", + "-z", + "0.1", + "-Y", + "1.57", + ], + ) + # Spawn robot in Gazebo Sim gazebo_spawn_robot = Node( package="ros_gz_sim", @@ -410,6 +450,7 @@ def generate_launch_description(): return LaunchDescription( [ # Launch arguments + set_gz_resource_path, declare_use_sim_time, declare_rviz, declare_robot_name, @@ -427,6 +468,7 @@ def generate_launch_description(): gz_sim, robot_state_publisher_node, gazebo_spawn_robot, + gazebo_spawn_aruco, gazebo_bridge, # CHAMP controller nodes quadruped_controller_node, diff --git a/go2_sdk/config/nav2_params.yaml b/go2_sdk/config/nav2_params.yaml index 5f9fded..75eb5c8 100644 --- a/go2_sdk/config/nav2_params.yaml +++ b/go2_sdk/config/nav2_params.yaml @@ -30,7 +30,7 @@ amcl: save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: True - transform_tolerance: 0.5 # Reduced from 1.0 + transform_tolerance: 1.0 update_min_a: 0.005 update_min_d: 0.02 z_hit: 0.85 @@ -48,11 +48,11 @@ bt_navigator: ros__parameters: use_sim_time: False global_frame: map - transform_tolerance: 0.5 # Reduced from 1.0 + transform_tolerance: 1.0 robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 - default_server_timeout: 20 + default_server_timeout: 30 enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 @@ -107,88 +107,85 @@ bt_navigator: controller_server: ros__parameters: use_sim_time: False - controller_frequency: 40.0 - min_x_velocity_threshold: 0.001 + controller_frequency: 30.0 + min_x_velocity_threshold: 0.05 min_y_velocity_threshold: 0.001 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.5 + min_theta_velocity_threshold: 0.1 + failure_tolerance: 0.8 progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] controller_plugins: ["FollowPath"] progress_checker: plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.3 # Increased slightly - movement_time_allowance: 20.0 # Increased from 15.0 + required_movement_radius: 0.1 + movement_time_allowance: 45.0 general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 + xy_goal_tolerance: 0.35 + yaw_goal_tolerance: 0.35 - # MPPI Controller - optimized for Unitree Go2 FollowPath: plugin: "nav2_mppi_controller::MPPIController" - time_steps: 56 # Prediction horizon steps - model_dt: 0.05 # Time between steps (50ms) - batch_size: 2000 - vx_std: 0.2 # Std dev for forward velocity sampling - vy_std: 0.0 # No lateral movement for quadruped - wz_std: 0.4 # Std dev for rotational velocity sampling - vx_max: 0.4 # Max forward velocity - vx_min: -0.3 # Max backward velocity + time_steps: 48 + model_dt: 0.07 + batch_size: 1500 + vx_std: 0.15 # Velocity variance (exploration noise) + vy_std: 0.0 + wz_std: 0.35 # Angular velocity variance + vx_max: 0.25 # Absolute max forward speed (m/s) + vx_min: -0.22 # Max reverse speed (m/s) vy_max: 0.0 - wz_max: 1.2 # Max rotation rate - iteration_count: 1 # MPPI iterations per control cycle - prune_distance: 1.7 # Distance ahead to prune path - transform_tolerance: 0.3 - temperature: 0.3 # Lower = more exploitation vs exploration - gamma: 0.015 # Discount factor - motion_model: "DiffDrive" # Use DiffDrive for Go2 - visualize: False # Set to True for debugging - regenerate_noises: False - - # Cost function weights (tune these for your needs) - critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] - - ConstraintCritic: + wz_max: 0.8 # Max angular velocity (rad/s) + iteration_count: 1 # Optimization iterations (1 is efficient) + prune_distance: 1.5 # Remove path points closer than this + transform_tolerance: 0.5 + temperature: 0.4 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: False + regenerate_noises: True + + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic", "ObstaclesCritic"] + + ConstraintCritic: # Penalizes violating velocity/accel limits enabled: True cost_power: 1 cost_weight: 4.0 - ObstaclesCritic: + GoalCritic: # Penalizes distance to goal enabled: True cost_power: 1 - cost_weight: 20.0 - consider_footprint: False - collision_cost: 10000.0 - collision_margin_distance: 0.1 - near_goal_distance: 0.5 + cost_weight: 10.0 + threshold_to_consider: 1.2 - GoalCritic: + ObstaclesCritic: # Penalizes collisions (CRITICAL) enabled: True cost_power: 1 - cost_weight: 5.0 # High weight for reaching goal - threshold_to_consider: 1.4 + cost_weight: 18.0 + consider_footprint: False + collision_cost: 10000.0 + collision_margin_distance: 0.12 + near_goal_distance: 0.5 GoalAngleCritic: enabled: True cost_power: 1 - cost_weight: 15.0 # Important for final orientation - threshold_to_consider: 0.5 + cost_weight: 10.0 + threshold_to_consider: 0.6 PreferForwardCritic: enabled: True cost_power: 1 - cost_weight: 1.0 # Penalize backward motion + cost_weight: 3.0 threshold_to_consider: 0.5 - # Obstacle and path following - CostCritic: + CostCritic: # Penalizes high-cost areas enabled: True cost_power: 1 - cost_weight: 3.81 + cost_weight: 3.5 critical_cost: 300.0 consider_footprint: True collision_cost: 1000000.0 @@ -198,52 +195,52 @@ controller_server: PathAlignCritic: enabled: True cost_power: 1 - cost_weight: 14.0 - max_path_occupancy_ratio: 0.05 - trajectory_point_step: 4 - threshold_to_consider: 0.5 - offset_from_furthest: 20 + cost_weight: 32.0 + max_path_occupancy_ratio: 0.07 + trajectory_point_step: 3 + threshold_to_consider: 0.6 + offset_from_furthest: 15 use_path_orientations: False PathFollowCritic: enabled: True cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 + cost_weight: 20.0 + offset_from_furthest: 6 + threshold_to_consider: 1.2 PathAngleCritic: enabled: True cost_power: 1 cost_weight: 2.0 offset_from_furthest: 4 - threshold_to_consider: 0.5 - max_angle_to_furthest: 1.0 + threshold_to_consider: 0.6 + max_angle_to_furthest: 1.2 mode: 0 - # Noise generator settings noise_generator_config: default: - time_steps: 56 - batch_size: 2000 - vx_std: 0.2 - wz_std: 0.4 + time_steps: 48 + batch_size: 1500 + vx_std: 0.15 + wz_std: 0.35 vx_mu: 0.0 wz_mu: 0.0 local_costmap: local_costmap: ros__parameters: - update_frequency: 10.0 - publish_frequency: 5.0 + update_frequency: 8.0 + publish_frequency: 4.0 global_frame: odom robot_base_frame: base_link use_sim_time: False rolling_window: True - width: 3 - height: 3 + width: 4 + height: 4 resolution: 0.05 - footprint: "[[0.3, 0.2], [0.3, -0.2], [-0.3, -0.2], [-0.3, 0.2]]" + transform_tolerance: 1.0 + footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" plugins: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] static_layer: @@ -253,92 +250,95 @@ local_costmap: obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True + footprint_clearing_enabled: True # Clear footprint area + max_obstacle_height: 2.0 observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 + min_obstacle_height: 0.0 clearing: True marking: True data_type: "LaserScan" - raytrace_max_range: 3.0 + raytrace_max_range: 3.5 raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 + obstacle_max_range: 3.0 + obstacle_min_range: 0.15 - # STVL for 4D LiDAR pointcloud (optimized for ground obstacles) stvl_layer: plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" enabled: True - voxel_decay: 0.8 # Faster decay for dynamic ground obstacles - decay_model: 0 # 0=linear, 1=exponential, 2=persistent - voxel_size: 0.04 # 4cm voxels for better ground detail - track_unknown_space: True + voxel_decay: 1.5 # Faster decay + decay_model: 0 + voxel_size: 0.05 + track_unknown_space: False observation_persistence: 0.0 - max_obstacle_height: 2.5 # Full height detection with 4D LiDAR - min_obstacle_height: 0.15 # 15.0cm - lower threshold for ground obstacles - obstacle_max_range: 2.5 # Focused range for ground detection - obstacle_min_range: 0.25 # Minimum range to avoid self-detection + max_obstacle_height: 2.5 + min_obstacle_height: 0.18 # Slightly higher to avoid ground noise + obstacle_max_range: 2.5 + obstacle_min_range: 0.25 origin_z: 0.0 publish_voxel_map: True - transform_tolerance: 0.5 + transform_tolerance: 1.0 mapping_mode: False map_save_duration: 60.0 - combination_method: 1 # 0=max, 1=override + combination_method: 1 update_footprint_enabled: True - - # 4D LiDAR as observation source observation_sources: lidar_4d - - # 4D LiDAR pointcloud configuration (Unitree Go2) lidar_4d: data_type: PointCloud2 - topic: /utlidar/cloud_deskewed # Motion-compensated pointcloud + topic: /utlidar/cloud_deskewed marking: True clearing: True - min_obstacle_height: 0.15 # 15.0cm threshold for ground obstacles - max_obstacle_height: 2.5 # Full height coverage - expected_update_rate: 15.0 # Unitree 4D LiDAR at ~15Hz + min_obstacle_height: 0.18 + max_obstacle_height: 2.5 + expected_update_rate: 15.0 observation_persistence: 0.0 inf_is_valid: False filter: "voxel" - voxel_min_points: 1 # Lower threshold for ground obstacle sensitivity + voxel_min_points: 3 clear_after_reading: True enabled: True - model_type: 1 # 1=3d_lidar (for 4D LiDAR) + model_type: 1 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 5.0 - inflation_radius: 0.25 + cost_scaling_factor: 3.5 # Decay rate (lower = larger danger zone) + inflation_radius: 0.25 # Radius to inflate costs around obstacles (m) + always_send_full_costmap: True global_costmap: global_costmap: ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 + update_frequency: 2.0 # Reduced + publish_frequency: 1.0 global_frame: map robot_base_frame: base_link use_sim_time: False - footprint: "[[0.3, 0.2], [0.3, -0.2], [-0.3, -0.2], [-0.3, 0.2]]" + transform_tolerance: 1.0 + footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" resolution: 0.1 - track_unknown_space: True + track_unknown_space: False plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True + footprint_clearing_enabled: True + max_obstacle_height: 2.0 observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 + min_obstacle_height: 0.0 clearing: True marking: True data_type: "LaserScan" - raytrace_max_range: 3.0 + raytrace_max_range: 4.0 raytrace_min_range: 0.0 obstacle_max_range: 3.5 - obstacle_min_range: 0.0 + obstacle_min_range: 0.15 static_layer: plugin: "nav2_costmap_2d::StaticLayer" @@ -346,8 +346,8 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 5.0 - inflation_radius: 0.30 + cost_scaling_factor: 3.5 # Decay rate + inflation_radius: 0.3 # Radius to inflate costs around obstacles (m) always_send_full_costmap: True @@ -367,19 +367,20 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 5.0 - transform_tolerance: 0.1 + transform_tolerance: 1.0 use_sim_time: False planner_plugins: ["GridBased"] - # Using SmacPlanner2D for better orientation handling GridBased: plugin: "nav2_smac_planner/SmacPlanner2D" - tolerance: 0.3 + tolerance: 0.5 downsample_costmap: False downsampling_factor: 1 - allow_unknown: True + allow_unknown: True # Allow planning through unknown space max_iterations: 1000000 max_on_approach_iterations: 1000 smooth_path: True + #Add cost_travel_multiplier to prefer free space + cost_travel_multiplier: 2.0 # Higher = prefer low cost areas vs short path smoother_server: ros__parameters: @@ -401,9 +402,9 @@ behavior_server: plugin: "nav2_behaviors/Spin" backup: plugin: "nav2_behaviors/BackUp" - backup_dist: 0.5 - backup_speed: 0.15 - time_allowance: 10.0 + backup_dist: 0.4 + backup_speed: 0.12 + time_allowance: 20.0 drive_on_heading: plugin: "nav2_behaviors/DriveOnHeading" wait: @@ -412,12 +413,12 @@ behavior_server: plugin: "nav2_behaviors/AssistedTeleop" global_frame: odom robot_base_frame: base_link - transform_tolerance: 0.5 + transform_tolerance: 1.0 # Increased use_sim_time: False - simulate_ahead_time: 2.5 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 + simulate_ahead_time: 3.0 + max_rotational_vel: 0.6 + min_rotational_vel: 0.15 + rotational_acc_lim: 1.8 robot_state_publisher: ros__parameters: @@ -440,10 +441,10 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.4, 0.0, 1.2] # Updated for MPPI limits - min_velocity: [-0.3, 0.0, -1.2] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] + max_velocity: [0.26, 0.0, 0.8] # Kinematic hard limits [vx, vy, wz] + min_velocity: [-0.26, 0.0, -0.8] + max_accel: [1.3, 0.0, 2.0] # Max acceleration [ax, ay, az] + max_decel: [-1.3, 0.0, -2.0] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] diff --git a/go2_sdk/go2_sdk/go2_dock.py b/go2_sdk/go2_sdk/go2_dock.py deleted file mode 100644 index 26feab2..0000000 --- a/go2_sdk/go2_sdk/go2_dock.py +++ /dev/null @@ -1,106 +0,0 @@ -#!/usr/bin/env python3 - -import subprocess - -import rclpy -from rclpy.node import Node - -from unitree_go.msg import LowState - - -class Go2AutoChargeMonitor(Node): - def __init__(self): - super().__init__("go2_auto_charge_monitor") - self.get_logger().info("Go2 Auto Charge Monitor node has been started.") - - self.subscription = self.create_subscription( - LowState, "/lf/lowstate", self.listener_callback, 10 - ) - - # State tracking - self.low_battery_threshold = 53.0 # Percentage - self.charging_navigation_triggered = False - self.is_charging = False - self.last_soc = None - - # Timer to periodically check battery status (every 10 seconds) - self.timer = self.create_timer(10.0, self.periodic_check) - - def listener_callback(self, msg): - """Process incoming battery data""" - soc = msg.bms_state.soc # State of charge (battery percentage) - current = msg.bms_state.current # Current in mA (positive = charging) - - # Update charging status - self.is_charging = current > 0 - self.last_soc = soc - - # Only log battery status if navigation hasn't been triggered - if not self.charging_navigation_triggered: - charging_status = "CHARGING" if self.is_charging else "DISCHARGING" - self.get_logger().info( - f"Battery: {soc:.1f}% | Current: {current} mA | Status: {charging_status}" - ) - - # Check if we need to navigate to charger - if ( - soc < self.low_battery_threshold - and not self.is_charging - and not self.charging_navigation_triggered - ): - self.get_logger().warn( - f"Battery low ({soc:.1f}% < {self.low_battery_threshold}%) and not charging!" - ) - self.trigger_charging_navigation() - - # Reset trigger flag if battery is above threshold or charging - if soc >= self.low_battery_threshold or self.is_charging: - if self.charging_navigation_triggered: - self.get_logger().info( - "Battery recovered or charging detected. Resetting trigger." - ) - self.charging_navigation_triggered = False - - def periodic_check(self): - """Periodic status check""" - # Only print periodic updates if navigation hasn't been triggered - if self.last_soc is not None and not self.charging_navigation_triggered: - status = "CHARGING" if self.is_charging else "DISCHARGING" - self.get_logger().info( - f"[Periodic Check] Battery: {self.last_soc:.1f}% | Status: {status}" - ) - - def trigger_charging_navigation(self): - """Launch the navigation to charger script""" - self.get_logger().warn("=" * 60) - self.get_logger().warn("INITIATING AUTOMATIC CHARGING SEQUENCE!") - self.get_logger().warn("=" * 60) - - try: - # Launch the navigation script - subprocess.Popen(["ros2", "run", "my_camera_pkg", "go2_dock"]) - self.charging_navigation_triggered = True - self.get_logger().info( - "Navigation to charger script launched successfully." - ) - - except Exception as e: - self.get_logger().error(f"Failed to launch charging navigation: {str(e)}") - self.charging_navigation_triggered = False - - -def main(args=None): - rclpy.init(args=args) - monitor = Go2AutoChargeMonitor() - - try: - rclpy.spin(monitor) - except KeyboardInterrupt: - monitor.get_logger().info("Keyboard interrupt detected, shutting down...") - finally: - monitor.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/go2_sdk/launch/go2_charge.launch.py b/go2_sdk/launch/go2_charge.launch.py index 974a9d9..f1d102f 100644 --- a/go2_sdk/launch/go2_charge.launch.py +++ b/go2_sdk/launch/go2_charge.launch.py @@ -3,14 +3,14 @@ def generate_launch_description(): - go2_charging_node = Node( - package="my_camera_pkg", - executable="go2_dock", - name="dock_charge_node", + go2_battery_monitor_node = Node( + package="go2_auto_dock", + executable="go2_battery_monitor", + name="go2_battery_monitor", output="screen", ) return LaunchDescription( [ - go2_charging_node, + go2_battery_monitor_node, ] ) diff --git a/go2_sdk/launch/nav2_launch.py b/go2_sdk/launch/nav2_launch.py index 0aa07cc..1f97c61 100644 --- a/go2_sdk/launch/nav2_launch.py +++ b/go2_sdk/launch/nav2_launch.py @@ -7,6 +7,47 @@ from launch.substitutions import EnvironmentVariable, LaunchConfiguration from launch_ros.actions import Node +SIM_PARAM_OVERRIDES = { + "controller_server": { + "FollowPath.vx_std": 0.2, + "FollowPath.wz_std": 0.4, + "FollowPath.vx_max": 0.5, + "FollowPath.vx_min": -0.35, + "FollowPath.wz_max": 1.2, + "FollowPath.ObstaclesCritic.cost_weight": 10.0, + "FollowPath.ObstaclesCritic.collision_margin_distance": 0.05, + "FollowPath.ObstaclesCritic.near_goal_distance": 0.4, + "FollowPath.GoalAngleCritic.cost_power": 4, + "FollowPath.PreferForwardCritic.cost_weight": 16.0, + # local costmap params + "inflation_layer.cost_scaling_factor": 5.0, + "inflation_layer.inflation_radius": 0.30, + }, + "planner_server": { + # global costmap params + "inflation_layer.cost_scaling_factor": 6.0, + "inflation_layer.inflation_radius": 0.32, + }, + "velocity_smoother": { + "max_velocity": [0.5, 0.0, 1.2], + "min_velocity": [-0.35, 0.0, -1.2], + "max_accel": [2.0, 0.0, 2.5], + "max_decel": [-2.0, 0.0, -2.5], + }, +} + + +def get_node_params(node_name: str, base_config: str, use_sim: bool): + """ + Merge base config with simulation overrides if needed. + """ + params = [base_config, {"use_sim_time": use_sim}] + + if use_sim and node_name in SIM_PARAM_OVERRIDES: + params.append(SIM_PARAM_OVERRIDES[node_name]) + + return params + def generate_launch_description(): pkg_dir = get_package_share_directory("go2_sdk") @@ -249,7 +290,9 @@ def generate_launch_description(): package="nav2_controller", executable="controller_server", output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], + parameters=get_node_params( + "controller_server", nav2_config_file, use_sim + ), remappings=[("/cmd_vel", "/cmd_vel")], ), Node( @@ -264,7 +307,7 @@ def generate_launch_description(): executable="planner_server", name="planner_server", output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], + parameters=get_node_params("planner_server", nav2_config_file, use_sim), ), Node( package="nav2_behaviors", diff --git a/go2_sdk/launch/slam_launch.py b/go2_sdk/launch/slam_launch.py index 0f8dc8f..510bca4 100644 --- a/go2_sdk/launch/slam_launch.py +++ b/go2_sdk/launch/slam_launch.py @@ -7,6 +7,47 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +SIM_PARAM_OVERRIDES = { + "controller_server": { + "FollowPath.vx_std": 0.2, + "FollowPath.wz_std": 0.4, + "FollowPath.vx_max": 0.5, + "FollowPath.vx_min": -0.35, + "FollowPath.wz_max": 1.2, + "FollowPath.ObstaclesCritic.cost_weight": 10.0, + "FollowPath.ObstaclesCritic.collision_margin_distance": 0.05, + "FollowPath.ObstaclesCritic.near_goal_distance": 0.4, + "FollowPath.GoalAngleCritic.cost_power": 4, + "FollowPath.PreferForwardCritic.cost_weight": 16.0, + # local costmap params + "inflation_layer.cost_scaling_factor": 5.0, + "inflation_layer.inflation_radius": 0.30, + }, + "planner_server": { + # global costmap params + "inflation_layer.cost_scaling_factor": 6.0, + "inflation_layer.inflation_radius": 0.32, + }, + "velocity_smoother": { + "max_velocity": [0.5, 0.0, 1.2], + "min_velocity": [-0.35, 0.0, -1.2], + "max_accel": [2.0, 0.0, 2.5], + "max_decel": [-2.0, 0.0, -2.5], + }, +} + + +def get_node_params(node_name: str, base_config: str, use_sim: bool): + """ + Merge base config with simulation overrides if needed. + """ + params = [base_config, {"use_sim_time": use_sim}] + + if use_sim and node_name in SIM_PARAM_OVERRIDES: + params.append(SIM_PARAM_OVERRIDES[node_name]) + + return params + def generate_launch_description(): pkg_dir = get_package_share_directory("go2_sdk") @@ -219,7 +260,9 @@ def generate_launch_description(): package="nav2_controller", executable="controller_server", output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], + parameters=get_node_params( + "controller_server", nav2_config_file, use_sim + ), remappings=[ ("/cmd_vel", "/cmd_vel"), ], @@ -236,7 +279,7 @@ def generate_launch_description(): executable="planner_server", name="planner_server", output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], + parameters=get_node_params("planner_server", nav2_config_file, use_sim), ), Node( package="nav2_behaviors", @@ -264,7 +307,9 @@ def generate_launch_description(): executable="velocity_smoother", name="velocity_smoother", output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], + parameters=get_node_params( + "velocity_smoother", nav2_config_file, use_sim + ), remappings=[ ("/cmd_vel", "/cmd_vel_nav"), ("/cmd_vel_smoothed", "/cmd_vel"),