├── .gitignore ├── README.md ├── nav.repos ├── unitree_nav ├── CMakeLists.txt ├── config │ ├── nav.rviz │ └── nav2_params.yaml ├── launch │ ├── control.launch.py │ ├── mapping.launch.py │ ├── rslidar_robosense.launch.py │ └── unitree_nav.launch.py ├── package.xml └── src │ ├── cmd_processor.cpp │ └── nav_to_pose.cpp ├── unitree_nav_interfaces ├── CMakeLists.txt ├── package.xml └── srv │ ├── NavToPose.srv │ ├── SetBodyRPY.srv │ └── SetGait.srv └── unitree_nav_launch_module ├── package.xml ├── resource └── unitree_nav_launch_module ├── setup.cfg ├── setup.py ├── test ├── test_copyright.py ├── test_flake8.py └── test_pep257.py └── unitree_nav_launch_module ├── __init__.py └── ternary_text_substitution.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | build/ 35 | 36 | # Python 37 | __pycache__/ 38 | *.py~ 39 | *.[#]file 40 | *.pyc 41 | 42 | .vscode/ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS 2 Unitree Go1 Nav2 Integration 2 | This repository has code and launch files for running the Unitree Go1 in high level mode with ROS 2 Humble, the RoboSense RS-Helios-16P it comes with, RTAB-Map, and the Nav2 stack. 3 | 4 | # Dependencies: 5 | Dependencies for this repository are listed in the `nav.repos` file. To import all dependencies, clone this repository into the `src` directory in your workspace root. Then from the workspace root directory, run the following commands: 6 | 7 | ``` 8 | vcs import < src/unitree_nav/nav.repos 9 | cd src/rslidar_sdk_ros2 10 | git submodule init 11 | git submodule update 12 | ``` 13 | 14 | # Launch Files 15 | Use `ros2 launch unitree_nav ${launch_file_name} --show-args` to view arguments for each launch file. 16 | 17 | - `unitree_nav.launch.py` - launches everything necessary for controlling the Unitree Go1 with the Nav2 stack, including control and mapping nodes. 18 | - `control.launch.py` - launches everything needed for ROS 2 control of the Unitree Go1 with services and `cmd_vel` publishing. 19 | - `mapping.launch.py` - launches RS LiDAR and RTAB-Map nodes to map with point cloud data from the RS-Helios-16P. 20 | - `rslidar_robosense.launch.py` - launches only RTAB-Map nodes set up in a configuration that is compatiable with the RS-Helios-16P. 21 | 22 | # Nodes 23 | ## cmd_processor 24 | This node handles commands and converts them into `HighCmd` messages which can be read by the high level UDP node in [this repository](https://github.com/katie-hughes/unitree_ros2) and then sent to the Go1 for motion control. 25 | 26 | It processes `geometry_msgs/Twist` messages sent on the `cmd_vel` topic to cause the Go1 to move. It also provides several services to trigger built in functions on the Go1. 27 | 28 | ### Services 29 | - `reset_state` - resets the commanded state to `IDLE`. 30 | - `stand_up` - commands the Go1 to stand up. 31 | - `recover_stand` - commands the Go1 to stand up, including from a fallen state. This is preferred over the `stand_up` service. 32 | - `lay_down` - commands the Go1 to lay down. 33 | - `damping` - puts the Go1's motors into "damping" mode. **Use caution with this service** - do NOT call this service while the robot is standing unsupported, it will cause it to fall. 34 | - `jump_yaw` - commands the Go1 to jump 90° counterclockwise. **Use caution with this service**. 35 | - `beg` - commands the Go1 to jump back into a begging stance. **Use caution with this service**. 36 | - `dance1` - commands the Go1 to perform its first dance. **Use caution with this service**. 37 | - `dance2` - commands the Go1 to perform its second dance. **Use caution with this service**. 38 | - `set_body_rpy` - set the values for the roll, pitch, and yaw of the body in radians. **Use caution with this service** - specifying values that are too large may make the robot fall over. Consult the suggested limits specified in [this file](unitree_nav_interfaces/srv/SetBodyRPY.srv). 39 | 40 | ## nav_to_pose 41 | This node is an example for working with the Nav2 stack to command the Go1 to a certain pose in the map. It can be used as an example for a custom node or, for basic use, can be ran and its services used. 42 | 43 | ### Services 44 | - `unitree_nav_to_pose` - begins action client sequence to navigate the Go1 to a specified pose. 45 | - `unitree_cancel_nav` - cancels all navigation goals. 46 | -------------------------------------------------------------------------------- /nav.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | src/rslidar_msg: 3 | type: git 4 | url: git@github.com:RoboSense-LiDAR/rslidar_msg.git 5 | version: master 6 | src/rslidar_sdk_ros2: 7 | type: git 8 | url: git@github.com:Marnonel6/rslidar_sdk_ros2.git 9 | version: unitree_go1 10 | src/unitree_nav: 11 | type: git 12 | url: git@github.com:ngmor/unitree_nav.git 13 | version: main 14 | src/unitree_ros2: 15 | type: git 16 | url: git@github.com:katie-hughes/unitree_ros2.git 17 | version: main 18 | -------------------------------------------------------------------------------- /unitree_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(unitree_nav) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(rclcpp_action REQUIRED) 12 | find_package(std_srvs REQUIRED) 13 | find_package(geometry_msgs REQUIRED) 14 | find_package(tf2 REQUIRED) 15 | find_package(tf2_geometry_msgs REQUIRED) 16 | find_package(nav2_msgs REQUIRED) 17 | find_package(ros2_unitree_legged_msgs REQUIRED) 18 | find_package(unitree_nav_interfaces REQUIRED) 19 | 20 | # executables 21 | add_executable(cmd_processor src/cmd_processor.cpp) 22 | ament_target_dependencies(cmd_processor 23 | rclcpp std_srvs geometry_msgs ros2_unitree_legged_msgs unitree_nav_interfaces 24 | ) 25 | 26 | add_executable(nav_to_pose src/nav_to_pose.cpp) 27 | ament_target_dependencies(nav_to_pose 28 | rclcpp rclcpp_action std_srvs geometry_msgs tf2 tf2_geometry_msgs nav2_msgs unitree_nav_interfaces 29 | ) 30 | 31 | install(TARGETS 32 | cmd_processor nav_to_pose 33 | DESTINATION lib/${PROJECT_NAME} 34 | ) 35 | 36 | install(DIRECTORY 37 | launch config 38 | DESTINATION share/${PROJECT_NAME} 39 | ) 40 | 41 | if(BUILD_TESTING) 42 | find_package(ament_lint_auto REQUIRED) 43 | # the following line skips the linter which checks for copyrights 44 | # comment the line when a copyright and license is added to all source files 45 | set(ament_cmake_copyright_FOUND TRUE) 46 | # the following line skips cpplint (only works in a git repo) 47 | # comment the line when this package is in a git repo and when 48 | # a copyright and license is added to all source files 49 | set(ament_cmake_cpplint_FOUND TRUE) 50 | ament_lint_auto_find_test_dependencies() 51 | endif() 52 | 53 | ament_package() 54 | -------------------------------------------------------------------------------- /unitree_nav/config/nav.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | Splitter Ratio: 0.5 11 | Tree Height: 640 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Goal Pose1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz_common/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: PointCloud2 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz_default_plugins/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.029999999329447746 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz_default_plugins/RobotModel 53 | Collision Enabled: false 54 | Description File: "" 55 | Description Source: Topic 56 | Description Topic: 57 | Depth: 5 58 | Durability Policy: Volatile 59 | History Policy: Keep Last 60 | Reliability Policy: Reliable 61 | Value: /robot_description 62 | Enabled: true 63 | Links: 64 | All Links Enabled: true 65 | Expand Joint Details: false 66 | Expand Link Details: false 67 | Expand Tree: false 68 | FL_calf: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | FL_calf_rotor: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | FL_foot: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | FL_hip: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | FL_hip_rotor: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | FL_thigh: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | FL_thigh_rotor: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | FR_calf: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | FR_calf_rotor: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | FR_foot: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | FR_hip: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | FR_hip_rotor: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | FR_thigh: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | FR_thigh_rotor: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Value: true 138 | Link Tree Style: Links in Alphabetic Order 139 | RL_calf: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Value: true 144 | RL_calf_rotor: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | Value: true 149 | RL_foot: 150 | Alpha: 1 151 | Show Axes: false 152 | Show Trail: false 153 | Value: true 154 | RL_hip: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | Value: true 159 | RL_hip_rotor: 160 | Alpha: 1 161 | Show Axes: false 162 | Show Trail: false 163 | Value: true 164 | RL_thigh: 165 | Alpha: 1 166 | Show Axes: false 167 | Show Trail: false 168 | Value: true 169 | RL_thigh_rotor: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | Value: true 174 | RR_calf: 175 | Alpha: 1 176 | Show Axes: false 177 | Show Trail: false 178 | Value: true 179 | RR_calf_rotor: 180 | Alpha: 1 181 | Show Axes: false 182 | Show Trail: false 183 | Value: true 184 | RR_foot: 185 | Alpha: 1 186 | Show Axes: false 187 | Show Trail: false 188 | Value: true 189 | RR_hip: 190 | Alpha: 1 191 | Show Axes: false 192 | Show Trail: false 193 | Value: true 194 | RR_hip_rotor: 195 | Alpha: 1 196 | Show Axes: false 197 | Show Trail: false 198 | Value: true 199 | RR_thigh: 200 | Alpha: 1 201 | Show Axes: false 202 | Show Trail: false 203 | Value: true 204 | RR_thigh_rotor: 205 | Alpha: 1 206 | Show Axes: false 207 | Show Trail: false 208 | Value: true 209 | base: 210 | Alpha: 1 211 | Show Axes: false 212 | Show Trail: false 213 | Value: true 214 | base_footprint: 215 | Alpha: 1 216 | Show Axes: false 217 | Show Trail: false 218 | base_laser: 219 | Alpha: 1 220 | Show Axes: false 221 | Show Trail: false 222 | base_link: 223 | Alpha: 1 224 | Show Axes: false 225 | Show Trail: false 226 | camera_chin: 227 | Alpha: 1 228 | Show Axes: false 229 | Show Trail: false 230 | Value: true 231 | camera_face: 232 | Alpha: 1 233 | Show Axes: false 234 | Show Trail: false 235 | Value: true 236 | camera_laserscan_link_left: 237 | Alpha: 1 238 | Show Axes: false 239 | Show Trail: false 240 | camera_laserscan_link_right: 241 | Alpha: 1 242 | Show Axes: false 243 | Show Trail: false 244 | camera_left: 245 | Alpha: 1 246 | Show Axes: false 247 | Show Trail: false 248 | Value: true 249 | camera_optical_chin: 250 | Alpha: 1 251 | Show Axes: false 252 | Show Trail: false 253 | camera_optical_face: 254 | Alpha: 1 255 | Show Axes: false 256 | Show Trail: false 257 | camera_optical_left: 258 | Alpha: 1 259 | Show Axes: false 260 | Show Trail: false 261 | camera_optical_rearDown: 262 | Alpha: 1 263 | Show Axes: false 264 | Show Trail: false 265 | camera_optical_right: 266 | Alpha: 1 267 | Show Axes: false 268 | Show Trail: false 269 | camera_rearDown: 270 | Alpha: 1 271 | Show Axes: false 272 | Show Trail: false 273 | Value: true 274 | camera_right: 275 | Alpha: 1 276 | Show Axes: false 277 | Show Trail: false 278 | Value: true 279 | height_measurement: 280 | Alpha: 1 281 | Show Axes: false 282 | Show Trail: false 283 | imu_link: 284 | Alpha: 1 285 | Show Axes: false 286 | Show Trail: false 287 | Value: true 288 | trunk: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | Value: true 293 | ultraSound_face: 294 | Alpha: 1 295 | Show Axes: false 296 | Show Trail: false 297 | Value: true 298 | ultraSound_left: 299 | Alpha: 1 300 | Show Axes: false 301 | Show Trail: false 302 | Value: true 303 | ultraSound_right: 304 | Alpha: 1 305 | Show Axes: false 306 | Show Trail: false 307 | Value: true 308 | Mass Properties: 309 | Inertia: false 310 | Mass: false 311 | Name: RobotModel 312 | TF Prefix: "" 313 | Update Interval: 0 314 | Value: true 315 | Visual Enabled: true 316 | - Class: rviz_default_plugins/TF 317 | Enabled: true 318 | Frame Timeout: 15 319 | Frames: 320 | All Enabled: false 321 | FL_calf: 322 | Value: false 323 | FL_calf_rotor: 324 | Value: false 325 | FL_foot: 326 | Value: false 327 | FL_hip: 328 | Value: false 329 | FL_hip_rotor: 330 | Value: false 331 | FL_thigh: 332 | Value: false 333 | FL_thigh_rotor: 334 | Value: false 335 | FR_calf: 336 | Value: false 337 | FR_calf_rotor: 338 | Value: false 339 | FR_foot: 340 | Value: false 341 | FR_hip: 342 | Value: false 343 | FR_hip_rotor: 344 | Value: false 345 | FR_thigh: 346 | Value: false 347 | FR_thigh_rotor: 348 | Value: false 349 | RL_calf: 350 | Value: false 351 | RL_calf_rotor: 352 | Value: false 353 | RL_foot: 354 | Value: false 355 | RL_hip: 356 | Value: false 357 | RL_hip_rotor: 358 | Value: false 359 | RL_thigh: 360 | Value: false 361 | RL_thigh_rotor: 362 | Value: false 363 | RR_calf: 364 | Value: false 365 | RR_calf_rotor: 366 | Value: false 367 | RR_foot: 368 | Value: false 369 | RR_hip: 370 | Value: false 371 | RR_hip_rotor: 372 | Value: false 373 | RR_thigh: 374 | Value: false 375 | RR_thigh_rotor: 376 | Value: false 377 | base: 378 | Value: true 379 | base_footprint: 380 | Value: true 381 | base_laser: 382 | Value: true 383 | base_link: 384 | Value: true 385 | camera_chin: 386 | Value: false 387 | camera_face: 388 | Value: false 389 | camera_laserscan_link_left: 390 | Value: false 391 | camera_laserscan_link_right: 392 | Value: false 393 | camera_left: 394 | Value: false 395 | camera_optical_chin: 396 | Value: false 397 | camera_optical_face: 398 | Value: false 399 | camera_optical_left: 400 | Value: false 401 | camera_optical_rearDown: 402 | Value: false 403 | camera_optical_right: 404 | Value: false 405 | camera_rearDown: 406 | Value: false 407 | camera_right: 408 | Value: false 409 | height_measurement: 410 | Value: true 411 | imu_link: 412 | Value: false 413 | map: 414 | Value: true 415 | odom: 416 | Value: true 417 | trunk: 418 | Value: false 419 | ultraSound_face: 420 | Value: false 421 | ultraSound_left: 422 | Value: false 423 | ultraSound_right: 424 | Value: false 425 | Marker Scale: 1 426 | Name: TF 427 | Show Arrows: true 428 | Show Axes: true 429 | Show Names: false 430 | Tree: 431 | map: 432 | {} 433 | odom: 434 | base_link: 435 | base: 436 | trunk: 437 | FL_hip: 438 | FL_thigh: 439 | FL_calf: 440 | FL_foot: 441 | {} 442 | FL_calf_rotor: 443 | {} 444 | FL_thigh_rotor: 445 | {} 446 | FL_hip_rotor: 447 | {} 448 | FR_hip: 449 | FR_thigh: 450 | FR_calf: 451 | FR_foot: 452 | {} 453 | FR_calf_rotor: 454 | {} 455 | FR_thigh_rotor: 456 | {} 457 | FR_hip_rotor: 458 | {} 459 | RL_hip: 460 | RL_thigh: 461 | RL_calf: 462 | RL_foot: 463 | {} 464 | RL_calf_rotor: 465 | {} 466 | RL_thigh_rotor: 467 | {} 468 | RL_hip_rotor: 469 | {} 470 | RR_hip: 471 | RR_thigh: 472 | RR_calf: 473 | RR_foot: 474 | {} 475 | RR_calf_rotor: 476 | {} 477 | RR_thigh_rotor: 478 | {} 479 | RR_hip_rotor: 480 | {} 481 | camera_chin: 482 | camera_optical_chin: 483 | {} 484 | camera_face: 485 | camera_optical_face: 486 | {} 487 | camera_left: 488 | camera_laserscan_link_left: 489 | {} 490 | camera_optical_left: 491 | {} 492 | camera_rearDown: 493 | camera_optical_rearDown: 494 | {} 495 | camera_right: 496 | camera_laserscan_link_right: 497 | {} 498 | camera_optical_right: 499 | {} 500 | imu_link: 501 | {} 502 | ultraSound_face: 503 | {} 504 | ultraSound_left: 505 | {} 506 | ultraSound_right: 507 | {} 508 | base_laser: 509 | {} 510 | height_measurement: 511 | base_footprint: 512 | {} 513 | Update Interval: 0 514 | Value: true 515 | - Alpha: 1 516 | Autocompute Intensity Bounds: true 517 | Autocompute Value Bounds: 518 | Max Value: 10 519 | Min Value: -10 520 | Value: true 521 | Axis: Z 522 | Channel Name: intensity 523 | Class: rviz_default_plugins/PointCloud2 524 | Color: 255; 255; 255 525 | Color Transformer: Intensity 526 | Decay Time: 0 527 | Enabled: true 528 | Invert Rainbow: false 529 | Max Color: 255; 255; 255 530 | Max Intensity: 188 531 | Min Color: 0; 0; 0 532 | Min Intensity: 1 533 | Name: PointCloud2 534 | Position Transformer: XYZ 535 | Selectable: true 536 | Size (Pixels): 3 537 | Size (m): 0.009999999776482582 538 | Style: Flat Squares 539 | Topic: 540 | Depth: 5 541 | Durability Policy: Volatile 542 | Filter size: 10 543 | History Policy: Keep Last 544 | Reliability Policy: Reliable 545 | Value: /rslidar_points 546 | Use Fixed Frame: true 547 | Use rainbow: true 548 | Value: true 549 | Enabled: true 550 | Global Options: 551 | Background Color: 48; 48; 48 552 | Fixed Frame: base 553 | Frame Rate: 30 554 | Name: root 555 | Tools: 556 | - Class: rviz_default_plugins/Interact 557 | Hide Inactive Objects: true 558 | - Class: rviz_default_plugins/MoveCamera 559 | - Class: rviz_default_plugins/Select 560 | - Class: rviz_default_plugins/FocusCamera 561 | - Class: rviz_default_plugins/Measure 562 | Line color: 128; 128; 0 563 | - Class: rviz_default_plugins/SetInitialPose 564 | Covariance x: 0.25 565 | Covariance y: 0.25 566 | Covariance yaw: 0.06853891909122467 567 | Topic: 568 | Depth: 5 569 | Durability Policy: Volatile 570 | History Policy: Keep Last 571 | Reliability Policy: Reliable 572 | Value: /initialpose 573 | - Class: rviz_default_plugins/SetGoal 574 | Topic: 575 | Depth: 5 576 | Durability Policy: Volatile 577 | History Policy: Keep Last 578 | Reliability Policy: Reliable 579 | Value: /goal_pose 580 | - Class: rviz_default_plugins/PublishPoint 581 | Single click: true 582 | Topic: 583 | Depth: 5 584 | Durability Policy: Volatile 585 | History Policy: Keep Last 586 | Reliability Policy: Reliable 587 | Value: /clicked_point 588 | Transformation: 589 | Current: 590 | Class: rviz_default_plugins/TF 591 | Value: true 592 | Views: 593 | Current: 594 | Class: rviz_default_plugins/Orbit 595 | Distance: 4.081317901611328 596 | Enable Stereo Rendering: 597 | Stereo Eye Separation: 0.05999999865889549 598 | Stereo Focal Distance: 1 599 | Swap Stereo Eyes: false 600 | Value: false 601 | Focal Point: 602 | X: -0.10900873690843582 603 | Y: -0.01570715382695198 604 | Z: -0.08127238601446152 605 | Focal Shape Fixed Size: true 606 | Focal Shape Size: 0.05000000074505806 607 | Invert Z Axis: false 608 | Name: Current View 609 | Near Clip Distance: 0.009999999776482582 610 | Pitch: 0.7153978943824768 611 | Target Frame: 612 | Value: Orbit (rviz) 613 | Yaw: 0.540402889251709 614 | Saved: ~ 615 | Window Geometry: 616 | Displays: 617 | collapsed: false 618 | Height: 946 619 | Hide Left Dock: false 620 | Hide Right Dock: false 621 | QMainWindow State: 000000ff00000000fd00000004000000000000016100000312fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000025f00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000312000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000312000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000040fc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005040000031200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 622 | Selection: 623 | collapsed: false 624 | Time: 625 | collapsed: false 626 | Tool Properties: 627 | collapsed: false 628 | Views: 629 | collapsed: false 630 | Width: 1920 631 | X: 0 632 | Y: 634 633 | -------------------------------------------------------------------------------- /unitree_nav/config/nav2_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "nav2_amcl::OmniMotionModel" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | 42 | bt_navigator: 43 | ros__parameters: 44 | use_sim_time: True 45 | global_frame: map 46 | robot_base_frame: base_link 47 | odom_topic: /odom 48 | bt_loop_duration: 10 49 | default_server_timeout: 20 50 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 51 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 52 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 53 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 54 | plugin_lib_names: 55 | - nav2_compute_path_to_pose_action_bt_node 56 | - nav2_compute_path_through_poses_action_bt_node 57 | - nav2_smooth_path_action_bt_node 58 | - nav2_follow_path_action_bt_node 59 | - nav2_spin_action_bt_node 60 | - nav2_wait_action_bt_node 61 | - nav2_assisted_teleop_action_bt_node 62 | - nav2_back_up_action_bt_node 63 | - nav2_drive_on_heading_bt_node 64 | - nav2_clear_costmap_service_bt_node 65 | - nav2_is_stuck_condition_bt_node 66 | - nav2_goal_reached_condition_bt_node 67 | - nav2_goal_updated_condition_bt_node 68 | - nav2_globally_updated_goal_condition_bt_node 69 | - nav2_is_path_valid_condition_bt_node 70 | - nav2_initial_pose_received_condition_bt_node 71 | - nav2_reinitialize_global_localization_service_bt_node 72 | - nav2_rate_controller_bt_node 73 | - nav2_distance_controller_bt_node 74 | - nav2_speed_controller_bt_node 75 | - nav2_truncate_path_action_bt_node 76 | - nav2_truncate_path_local_action_bt_node 77 | - nav2_goal_updater_node_bt_node 78 | - nav2_recovery_node_bt_node 79 | - nav2_pipeline_sequence_bt_node 80 | - nav2_round_robin_node_bt_node 81 | - nav2_transform_available_condition_bt_node 82 | - nav2_time_expired_condition_bt_node 83 | - nav2_path_expiring_timer_condition 84 | - nav2_distance_traveled_condition_bt_node 85 | - nav2_single_trigger_bt_node 86 | - nav2_goal_updated_controller_bt_node 87 | - nav2_is_battery_low_condition_bt_node 88 | - nav2_navigate_through_poses_action_bt_node 89 | - nav2_navigate_to_pose_action_bt_node 90 | - nav2_remove_passed_goals_action_bt_node 91 | - nav2_planner_selector_bt_node 92 | - nav2_controller_selector_bt_node 93 | - nav2_goal_checker_selector_bt_node 94 | - nav2_controller_cancel_bt_node 95 | - nav2_path_longer_on_approach_bt_node 96 | - nav2_wait_cancel_bt_node 97 | - nav2_spin_cancel_bt_node 98 | - nav2_back_up_cancel_bt_node 99 | - nav2_assisted_teleop_cancel_bt_node 100 | - nav2_drive_on_heading_cancel_bt_node 101 | 102 | bt_navigator_navigate_through_poses_rclcpp_node: 103 | ros__parameters: 104 | use_sim_time: True 105 | 106 | bt_navigator_navigate_to_pose_rclcpp_node: 107 | ros__parameters: 108 | use_sim_time: True 109 | 110 | controller_server: 111 | ros__parameters: 112 | use_sim_time: True 113 | controller_frequency: 20.0 114 | min_x_velocity_threshold: 0.001 115 | min_y_velocity_threshold: 0.001 116 | min_theta_velocity_threshold: 0.001 117 | failure_tolerance: 0.3 118 | progress_checker_plugin: "progress_checker" 119 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 120 | controller_plugins: ["FollowPath"] 121 | 122 | # Progress checker parameters 123 | progress_checker: 124 | plugin: "nav2_controller::SimpleProgressChecker" 125 | required_movement_radius: 0.5 126 | movement_time_allowance: 10.0 127 | # Goal checker parameters 128 | #precise_goal_checker: 129 | # plugin: "nav2_controller::SimpleGoalChecker" 130 | # xy_goal_tolerance: 0.25 131 | # yaw_goal_tolerance: 0.25 132 | # stateful: True 133 | general_goal_checker: 134 | stateful: True 135 | plugin: "nav2_controller::SimpleGoalChecker" 136 | xy_goal_tolerance: 0.1 137 | yaw_goal_tolerance: 0.25 138 | # DWB parameters 139 | FollowPath: 140 | plugin: "dwb_core::DWBLocalPlanner" 141 | debug_trajectory_details: True 142 | min_vel_x: -0.15 143 | min_vel_y: -0.15 144 | max_vel_x: 0.15 145 | max_vel_y: 0.15 146 | max_vel_theta: 1.0 147 | min_speed_xy: 0.0 148 | max_speed_xy: 0.15 149 | min_speed_theta: 0.0 150 | # Add high threshold velocity for turtlebot 3 issue. 151 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 152 | acc_lim_x: 2.5 153 | acc_lim_y: 2.5 154 | acc_lim_theta: 3.2 155 | decel_lim_x: -2.5 156 | decel_lim_y: -2.5 157 | decel_lim_theta: -3.2 158 | vx_samples: 20 159 | vy_samples: 20 160 | vtheta_samples: 20 161 | sim_time: 1.7 162 | linear_granularity: 0.05 163 | angular_granularity: 0.025 164 | transform_tolerance: 0.2 165 | xy_goal_tolerance: 0.1 166 | trans_stopped_velocity: 0.25 167 | short_circuit_trajectory_evaluation: True 168 | stateful: True 169 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 170 | BaseObstacle.scale: 0.02 171 | PathAlign.scale: 32.0 172 | PathAlign.forward_point_distance: 0.1 173 | GoalAlign.scale: 24.0 174 | GoalAlign.forward_point_distance: 0.1 175 | PathDist.scale: 32.0 176 | GoalDist.scale: 24.0 177 | RotateToGoal.scale: 32.0 178 | RotateToGoal.slowing_factor: 5.0 179 | RotateToGoal.lookahead_time: -1.0 180 | 181 | local_costmap: 182 | local_costmap: 183 | ros__parameters: 184 | update_frequency: 5.0 185 | publish_frequency: 2.0 186 | global_frame: odom 187 | robot_base_frame: base_link 188 | use_sim_time: True 189 | rolling_window: true 190 | width: 3 191 | height: 3 192 | resolution: 0.05 193 | footprint: "[ [0.410, 0.350], [0.410, -0.350], [-0.410, -0.350], [-0.410, 0.350] ]" 194 | plugins: ["voxel_layer", "inflation_layer"] 195 | inflation_layer: 196 | plugin: "nav2_costmap_2d::InflationLayer" 197 | cost_scaling_factor: 3.0 198 | inflation_radius: 0.55 199 | voxel_layer: 200 | plugin: "nav2_costmap_2d::VoxelLayer" 201 | enabled: True 202 | publish_voxel_map: True 203 | origin_z: 0.0 204 | z_resolution: 0.05 205 | z_voxels: 16 206 | max_obstacle_height: 2.0 207 | mark_threshold: 0 208 | observation_sources: scan3d 209 | scan: 210 | topic: /scan 211 | max_obstacle_height: 2.0 212 | clearing: True 213 | marking: True 214 | data_type: "LaserScan" 215 | raytrace_max_range: 3.0 216 | raytrace_min_range: 0.0 217 | obstacle_max_range: 2.5 218 | obstacle_min_range: 0.0 219 | scan3d: 220 | topic: /rslidar_points 221 | max_obstacle_height: 2.0 222 | clearing: True 223 | marking: True 224 | data_type: "PointCloud2" 225 | raytrace_max_range: 3.0 226 | raytrace_min_range: 0.0 227 | obstacle_max_range: 2.5 228 | obstacle_min_range: 0.0 229 | static_layer: 230 | plugin: "nav2_costmap_2d::StaticLayer" 231 | map_subscribe_transient_local: True 232 | always_send_full_costmap: True 233 | 234 | global_costmap: 235 | global_costmap: 236 | ros__parameters: 237 | update_frequency: 1.0 238 | publish_frequency: 1.0 239 | global_frame: map 240 | robot_base_frame: base_link 241 | use_sim_time: True 242 | footprint: "[ [0.410, 0.350], [0.410, -0.350], [-0.410, -0.350], [-0.410, 0.350] ]" 243 | resolution: 0.05 244 | track_unknown_space: true 245 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 246 | obstacle_layer: 247 | plugin: "nav2_costmap_2d::ObstacleLayer" 248 | enabled: True 249 | observation_sources: scan3d 250 | scan: 251 | topic: /scan 252 | max_obstacle_height: 2.0 253 | clearing: True 254 | marking: True 255 | data_type: "LaserScan" 256 | raytrace_max_range: 3.0 257 | raytrace_min_range: 0.0 258 | obstacle_max_range: 2.5 259 | obstacle_min_range: 0.0 260 | scan3d: 261 | topic: /rslidar_points 262 | max_obstacle_height: 2.0 263 | clearing: True 264 | marking: True 265 | data_type: "PointCloud2" 266 | raytrace_max_range: 3.0 267 | raytrace_min_range: 0.0 268 | obstacle_max_range: 2.5 269 | obstacle_min_range: 0.0 270 | static_layer: 271 | plugin: "nav2_costmap_2d::StaticLayer" 272 | map_subscribe_transient_local: True 273 | inflation_layer: 274 | plugin: "nav2_costmap_2d::InflationLayer" 275 | cost_scaling_factor: 3.0 276 | inflation_radius: 0.55 277 | always_send_full_costmap: True 278 | 279 | map_server: 280 | ros__parameters: 281 | use_sim_time: True 282 | # Overridden in launch by the "map" launch configuration or provided default value. 283 | # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. 284 | yaml_filename: "" 285 | 286 | map_saver: 287 | ros__parameters: 288 | use_sim_time: True 289 | save_map_timeout: 5.0 290 | free_thresh_default: 0.25 291 | occupied_thresh_default: 0.65 292 | map_subscribe_transient_local: True 293 | 294 | planner_server: 295 | ros__parameters: 296 | expected_planner_frequency: 20.0 297 | use_sim_time: True 298 | planner_plugins: ["GridBased"] 299 | GridBased: 300 | plugin: "nav2_navfn_planner/NavfnPlanner" 301 | tolerance: 0.5 302 | use_astar: false 303 | allow_unknown: true 304 | 305 | smoother_server: 306 | ros__parameters: 307 | use_sim_time: True 308 | smoother_plugins: ["simple_smoother"] 309 | simple_smoother: 310 | plugin: "nav2_smoother::SimpleSmoother" 311 | tolerance: 1.0e-10 312 | max_its: 1000 313 | do_refinement: True 314 | 315 | behavior_server: 316 | ros__parameters: 317 | costmap_topic: local_costmap/costmap_raw 318 | footprint_topic: local_costmap/published_footprint 319 | cycle_frequency: 10.0 320 | behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] 321 | spin: 322 | plugin: "nav2_behaviors/Spin" 323 | backup: 324 | plugin: "nav2_behaviors/BackUp" 325 | drive_on_heading: 326 | plugin: "nav2_behaviors/DriveOnHeading" 327 | wait: 328 | plugin: "nav2_behaviors/Wait" 329 | assisted_teleop: 330 | plugin: "nav2_behaviors/AssistedTeleop" 331 | global_frame: odom 332 | robot_base_frame: base_link 333 | transform_tolerance: 0.1 334 | use_sim_time: true 335 | simulate_ahead_time: 2.0 336 | max_rotational_vel: 0.6 337 | min_rotational_vel: 0.4 338 | rotational_acc_lim: 3.2 339 | 340 | robot_state_publisher: 341 | ros__parameters: 342 | use_sim_time: True 343 | 344 | waypoint_follower: 345 | ros__parameters: 346 | use_sim_time: True 347 | loop_rate: 20 348 | stop_on_failure: false 349 | waypoint_task_executor_plugin: "wait_at_waypoint" 350 | wait_at_waypoint: 351 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 352 | enabled: True 353 | waypoint_pause_duration: 200 354 | 355 | velocity_smoother: 356 | ros__parameters: 357 | use_sim_time: True 358 | smoothing_frequency: 20.0 359 | scale_velocities: False 360 | feedback: "OPEN_LOOP" 361 | max_velocity: [0.15, 0.15, 0.6] 362 | min_velocity: [-0.15, -0.15, -0.6] 363 | max_accel: [2.5, 2.5, 3.2] 364 | max_decel: [-2.5, -2.5, -3.2] 365 | odom_topic: "odom" 366 | odom_duration: 0.1 367 | deadband_velocity: [0.0, 0.0, 0.0] 368 | velocity_timeout: 1.0 369 | -------------------------------------------------------------------------------- /unitree_nav/launch/control.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 5 | from launch_ros.substitutions import FindPackageShare 6 | from launch.launch_description_sources import PythonLaunchDescriptionSource 7 | 8 | 9 | def generate_launch_description(): 10 | return LaunchDescription([ 11 | DeclareLaunchArgument( 12 | name='use_rviz', 13 | default_value='true', 14 | choices=['true','false'], 15 | description='Open RVIZ for Go1 visualization' 16 | ), 17 | 18 | IncludeLaunchDescription( 19 | PythonLaunchDescriptionSource( 20 | PathJoinSubstitution([ 21 | FindPackageShare('unitree_legged_real'), 22 | 'launch', 23 | 'high.launch.py' 24 | ]) 25 | ), 26 | launch_arguments=[ 27 | ('use_rviz', LaunchConfiguration('use_rviz')), 28 | ], 29 | ), 30 | Node( 31 | package='unitree_nav', 32 | executable='cmd_processor', 33 | output='screen' 34 | ), 35 | ]) -------------------------------------------------------------------------------- /unitree_nav/launch/mapping.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument 3 | from launch.substitutions import PathJoinSubstitution, LaunchConfiguration 4 | from launch.conditions import IfCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch_ros.actions import Node 7 | from launch_ros.substitutions import FindPackageShare 8 | 9 | 10 | 11 | def generate_launch_description(): 12 | return LaunchDescription([ 13 | 14 | DeclareLaunchArgument( 15 | name='publish_static_tf', 16 | default_value='true', 17 | choices=['true','false'], 18 | description='Publish a static transform between base_link and base_laser for standalone use of this launch file.' 19 | ), 20 | 21 | DeclareLaunchArgument( 22 | name='use_rtabmapviz', 23 | default_value='false', # suppress incessant VTK 9.0 warnings 24 | choices=['true','false'], 25 | description='Start rtabmapviz node' 26 | ), 27 | 28 | DeclareLaunchArgument( 29 | name='localize_only', 30 | default_value='true', 31 | choices=['true','false'], 32 | description='Localize only, do not change loaded map' 33 | ), 34 | 35 | DeclareLaunchArgument( 36 | name='restart_map', 37 | default_value='false', 38 | choices=['true','false'], 39 | description='Delete previous map and restart' 40 | ), 41 | 42 | DeclareLaunchArgument( 43 | name='icp_odometry_log_level', 44 | default_value='WARN', # reduce output from this node 45 | choices=['ERROR', 'WARN', 'INFO', 'DEBUG'], 46 | description='Set logger level for icp_odometry. Can set to WARN to reduce message output from this node.' 47 | ), 48 | 49 | DeclareLaunchArgument( 50 | name='use_rviz', 51 | default_value='true', 52 | choices=['true','false'], 53 | description='Open RVIZ for visualization' 54 | ), 55 | 56 | # Publish a static transform between base_link and base_laser for standalone use 57 | # of this launch file 58 | Node( 59 | package="tf2_ros", 60 | executable="static_transform_publisher", 61 | arguments=['--frame-id', 'base_link', '--child-frame-id', 'base_laser'], 62 | condition=IfCondition(LaunchConfiguration('publish_static_tf')) 63 | ), 64 | 65 | IncludeLaunchDescription( 66 | PythonLaunchDescriptionSource( 67 | PathJoinSubstitution([ 68 | FindPackageShare('rslidar_sdk'), 69 | 'launch', 70 | 'start.py' 71 | ]) 72 | ), 73 | launch_arguments=[ 74 | ('use_rviz', LaunchConfiguration('use_rviz')), 75 | ], 76 | ), 77 | IncludeLaunchDescription( 78 | PythonLaunchDescriptionSource( 79 | PathJoinSubstitution([ 80 | FindPackageShare('unitree_nav'), 81 | 'launch', 82 | 'rslidar_robosense.launch.py' 83 | ]) 84 | ), 85 | launch_arguments=[ 86 | ('use_rtabmapviz', LaunchConfiguration('use_rtabmapviz')), 87 | ('icp_odometry_log_level', LaunchConfiguration('icp_odometry_log_level')), 88 | ('localize_only', LaunchConfiguration('localize_only')), 89 | ('restart_map', LaunchConfiguration('restart_map')), 90 | ], 91 | ), 92 | ]) -------------------------------------------------------------------------------- /unitree_nav/launch/rslidar_robosense.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.conditions import IfCondition 4 | from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, TextSubstitution 5 | from launch_ros.actions import Node 6 | from launch_ros.substitutions import FindPackageShare 7 | from unitree_nav_launch_module import TernaryTextSubstitution 8 | 9 | def generate_launch_description(): 10 | return LaunchDescription([ 11 | 12 | # Launch arguments 13 | DeclareLaunchArgument( 14 | name='use_sim_time', 15 | default_value='false', 16 | choices=['true','false'], 17 | description='Use simulation (Gazebo) clock if true' 18 | ), 19 | 20 | DeclareLaunchArgument( 21 | name='deskewing', 22 | default_value='false', 23 | choices=['true','false'], 24 | description='Enable lidar deskewing' 25 | ), 26 | 27 | DeclareLaunchArgument( 28 | name='localize_only', 29 | default_value='true', 30 | choices=['true','false'], 31 | description='Localize only, do not change loaded map' 32 | ), 33 | 34 | DeclareLaunchArgument( 35 | name='restart_map', 36 | default_value='false', 37 | choices=['true','false'], 38 | description='Delete previous map and restart' 39 | ), 40 | 41 | DeclareLaunchArgument( 42 | name='use_rtabmapviz', 43 | default_value='true', 44 | choices=['true','false'], 45 | description='Start rtabmapviz node' 46 | ), 47 | 48 | DeclareLaunchArgument( 49 | name='rtabmap_log_level', 50 | default_value='INFO', 51 | choices=['ERROR', 'WARN', 'INFO', 'DEBUG'], 52 | description='Set logger level for rtabmap.' 53 | ), 54 | 55 | DeclareLaunchArgument( 56 | name='icp_odometry_log_level', 57 | default_value='INFO', 58 | choices=['ERROR', 'WARN', 'INFO', 'DEBUG'], 59 | description='Set logger level for icp_odometry. Can set to WARN to reduce message output from this node.' 60 | ), 61 | 62 | # Nodes to launch 63 | Node( 64 | package='rtabmap_odom', executable='icp_odometry', output='screen', 65 | parameters=[{ 66 | 'frame_id':'base_link', 67 | 'odom_frame_id':'odom', 68 | 'wait_for_transform':0.3, # 0.2 69 | 'expected_update_rate':15.0, 70 | 'deskewing':LaunchConfiguration('deskewing'), 71 | 'use_sim_time':LaunchConfiguration('use_sim_time'), 72 | }], 73 | remappings=[ 74 | ('scan_cloud', '/rslidar_points') 75 | ], 76 | arguments=[ 77 | 'Icp/PointToPlane', 'true', 78 | 'Icp/Iterations', '10', 79 | 'Icp/VoxelSize', '0.1', 80 | 'Icp/Epsilon', '0.001', 81 | 'Icp/PointToPlaneK', '20', 82 | 'Icp/PointToPlaneRadius', '0', 83 | 'Icp/MaxTranslation', '2', 84 | 'Icp/MaxCorrespondenceDistance', '1', 85 | 'Icp/Strategy', '1', 86 | 'Icp/OutlierRatio', '0.7', 87 | 'Icp/CorrespondenceRatio', '0.01', 88 | 'Odom/ScanKeyFrameThr', '0.6', 89 | 'OdomF2M/ScanSubtractRadius', '0.1', 90 | 'OdomF2M/ScanMaxSize', '15000', 91 | 'OdomF2M/BundleAdjustment', 'false', 92 | '--ros-args', 93 | '--log-level', 94 | [ 95 | TextSubstitution(text='icp_odometry:='), 96 | LaunchConfiguration('icp_odometry_log_level'), 97 | ], 98 | ] 99 | ), 100 | 101 | Node( 102 | package='rtabmap_util', executable='point_cloud_assembler', output='screen', 103 | parameters=[{ 104 | 'max_clouds':10, 105 | 'fixed_frame_id':'', 106 | 'use_sim_time':LaunchConfiguration('use_sim_time'), 107 | }], 108 | remappings=[ 109 | ('cloud', 'odom_filtered_input_scan') 110 | ]), 111 | 112 | Node( 113 | package='rtabmap_slam', executable='rtabmap', output='screen', 114 | parameters=[{ 115 | 'frame_id':'base_laser', 116 | 'subscribe_depth':False, 117 | 'subscribe_rgb':False, 118 | 'subscribe_scan_cloud':True, 119 | 'approx_sync':True, # False 120 | 'wait_for_transform': 0.3, #0.2, 121 | 'use_sim_time':LaunchConfiguration('use_sim_time'), 122 | }], 123 | remappings=[ 124 | ('scan_cloud', 'assembled_cloud') 125 | ], 126 | arguments=[ 127 | TernaryTextSubstitution(IfCondition(LaunchConfiguration('restart_map')), '-d', ''), 128 | 'Mem/IncrementalMemory', TernaryTextSubstitution(IfCondition(LaunchConfiguration('localize_only')), 'false', 'true'), 129 | 'Mem/InitWMWithAllNodes', LaunchConfiguration('localize_only'), 130 | 'RGBD/ProximityMaxGraphDepth', '0', 131 | 'RGBD/ProximityPathMaxNeighbors', '1', 132 | 'RGBD/AngularUpdate', '0.05', 133 | 'RGBD/LinearUpdate', '0.05', 134 | 'RGBD/CreateOccupancyGrid', 'false', 135 | 'Mem/NotLinkedNodesKept', 'false', 136 | 'Mem/STMSize', '30', 137 | 'Mem/LaserScanNormalK', '20', 138 | 'Reg/Strategy', '1', 139 | 'Icp/VoxelSize', '0.1', 140 | 'Icp/PointToPlaneK', '20', 141 | 'Icp/PointToPlaneRadius', '0', 142 | 'Icp/PointToPlane', 'true', 143 | 'Icp/Iterations', '10', 144 | 'Icp/Epsilon', '0.001', 145 | 'Icp/MaxTranslation', '3', 146 | 'Icp/MaxCorrespondenceDistance', '1', 147 | 'Icp/Strategy', '1', 148 | 'Icp/OutlierRatio', '0.7', 149 | 'Icp/CorrespondenceRatio', '0.2', 150 | '--ros-args', 151 | '--log-level', 152 | [ 153 | TextSubstitution(text='rtabmap:='), 154 | LaunchConfiguration('rtabmap_log_level'), 155 | ], 156 | ]), 157 | Node( 158 | package='rtabmap_viz', executable='rtabmap_viz', output='screen', 159 | parameters=[{ 160 | 'frame_id':'base_laser', 161 | 'odom_frame_id':'odom', 162 | 'subscribe_odom_info':True, 163 | 'subscribe_scan_cloud':True, 164 | 'approx_sync':True, # False 165 | 'use_sim_time':LaunchConfiguration('use_sim_time'), 166 | }], 167 | remappings=[ 168 | ('scan_cloud', 'odom_filtered_input_scan') 169 | ], 170 | condition=IfCondition(LaunchConfiguration('use_rtabmapviz')) 171 | ), 172 | ]) 173 | -------------------------------------------------------------------------------- /unitree_nav/launch/unitree_nav.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 5 | from launch.conditions import IfCondition 6 | from launch_ros.substitutions import FindPackageShare 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | 9 | 10 | def generate_launch_description(): 11 | return LaunchDescription([ 12 | 13 | DeclareLaunchArgument( 14 | name='use_rviz', 15 | default_value='true', 16 | choices=['true','false'], 17 | description='Open RVIZ for Go1 visualization' 18 | ), 19 | 20 | DeclareLaunchArgument( 21 | name='use_nav2_rviz', 22 | default_value='true', 23 | choices=['true','false'], 24 | description='Open RVIZ for Nav2 visualization' 25 | ), 26 | 27 | DeclareLaunchArgument( 28 | name='localize_only', 29 | default_value='true', 30 | choices=['true','false'], 31 | description='Localize only, do not change loaded map' 32 | ), 33 | 34 | DeclareLaunchArgument( 35 | name='restart_map', 36 | default_value='false', 37 | choices=['true','false'], 38 | description='Delete previous map and restart' 39 | ), 40 | 41 | Node( 42 | package='rviz2', 43 | executable='rviz2', 44 | arguments=[ 45 | '-d', 46 | PathJoinSubstitution([ 47 | FindPackageShare('unitree_nav'), 48 | 'config', 49 | 'nav.rviz' 50 | ]) 51 | ], 52 | condition=IfCondition(LaunchConfiguration('use_rviz')), 53 | ), 54 | 55 | IncludeLaunchDescription( 56 | PythonLaunchDescriptionSource( 57 | PathJoinSubstitution([ 58 | FindPackageShare('unitree_nav'), 59 | 'launch', 60 | 'control.launch.py' 61 | ]) 62 | ), 63 | launch_arguments=[ 64 | ('use_rviz', 'false'), 65 | ], 66 | ), 67 | 68 | IncludeLaunchDescription( 69 | PythonLaunchDescriptionSource( 70 | PathJoinSubstitution([ 71 | FindPackageShare('unitree_nav'), 72 | 'launch', 73 | 'mapping.launch.py' 74 | ]) 75 | ), 76 | launch_arguments=[ 77 | ('use_rviz', 'false'), 78 | ('publish_static_tf', 'false'), 79 | ('localize_only', LaunchConfiguration('localize_only')), 80 | ('restart_map', LaunchConfiguration('restart_map')), 81 | ], 82 | ), 83 | 84 | IncludeLaunchDescription( 85 | PythonLaunchDescriptionSource( 86 | PathJoinSubstitution([ 87 | FindPackageShare('nav2_bringup'), 88 | 'launch', 89 | 'navigation_launch.py' 90 | ]) 91 | ), 92 | launch_arguments=[ 93 | ('params_file', 94 | PathJoinSubstitution([ 95 | FindPackageShare('unitree_nav'), 96 | 'config', 97 | 'nav2_params.yaml' 98 | ]) 99 | ), 100 | ], 101 | ), 102 | 103 | IncludeLaunchDescription( 104 | PythonLaunchDescriptionSource( 105 | PathJoinSubstitution([ 106 | FindPackageShare('nav2_bringup'), 107 | 'launch', 108 | 'rviz_launch.py' 109 | ]) 110 | ), 111 | condition=IfCondition(LaunchConfiguration('use_nav2_rviz')), 112 | ), 113 | ]) -------------------------------------------------------------------------------- /unitree_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | unitree_nav 5 | 0.0.0 6 | Navigation control for Unitree Go1 7 | Nick Morales 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | ros2launch 13 | rclcpp 14 | rclcpp_action 15 | std_srvs 16 | geometry_msgs 17 | tf2 18 | tf2_geometry_msgs 19 | nav2_msgs 20 | ros2_unitree_legged_msgs 21 | unitree_nav_interfaces 22 | 23 | tf2_ros 24 | rtabmap 25 | rtabmap_ros 26 | rslidar_sdk 27 | 28 | ament_lint_auto 29 | ament_lint_common 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /unitree_nav/src/cmd_processor.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include "std_srvs/srv/empty.hpp" 3 | #include "geometry_msgs/msg/twist.hpp" 4 | #include "ros2_unitree_legged_msgs/msg/high_cmd.hpp" 5 | #include "unitree_nav_interfaces/srv/set_body_rpy.hpp" 6 | #include "unitree_nav_interfaces/srv/set_gait.hpp" 7 | 8 | using namespace std::chrono_literals; 9 | 10 | //TODO move into a shared library 11 | enum class Go1Mode : uint8_t 12 | { 13 | idle = 0 14 | ,force_stand = 1 15 | ,target_velocity_walking = 2 16 | ,target_position_walking = 3 17 | ,path_walking = 4 18 | ,position_stand_down = 5 19 | ,position_stand_up = 6 20 | ,damping = 7 21 | ,recovery_stand = 8 22 | ,backflip = 9 23 | ,jump_yaw = 10 24 | ,straight_hand = 11 25 | ,dance1 = 12 26 | ,dance2 = 13 27 | }; 28 | 29 | enum class Go1Gait : uint8_t 30 | { 31 | idle = 0 32 | ,trot = 1 33 | ,trot_running = 2 34 | ,climb_stairs = 3 35 | ,trot_obstacle = 4 36 | }; 37 | 38 | enum class Go1SpeedLevel : uint8_t 39 | { 40 | low = 0 41 | ,medium = 1 42 | ,high = 2 43 | }; 44 | 45 | //https://stackoverflow.com/questions/11421432/how-can-i-output-the-value-of-an-enum-class-in-c11 46 | template 47 | auto to_value(Enumeration const value) 48 | -> typename std::underlying_type::type 49 | { 50 | return static_cast::type>(value); 51 | } 52 | 53 | class CmdProcessor : public rclcpp::Node 54 | { 55 | public: 56 | CmdProcessor() 57 | : Node("cmd_processor") 58 | { 59 | //Parameters 60 | auto param = rcl_interfaces::msg::ParameterDescriptor{}; 61 | param.description = "The rate at which the node sends commands (Hz)."; 62 | declare_parameter("rate", 200.0, param); 63 | rate_ = get_parameter("rate").get_parameter_value().get(); 64 | interval_ = 1.0 / rate_; 65 | interval_ms_ = static_cast(static_cast(interval_ * 1000.0)); 66 | 67 | param.description = "The amount of time (ms) for which a received cmd_vel will be published."; 68 | declare_parameter("cmd_vel_timeout", 250, param); 69 | cmd_vel_timeout_ = static_cast( 70 | get_parameter("cmd_vel_timeout").get_parameter_value().get() 71 | ); 72 | 73 | //Timers 74 | timer_ = create_wall_timer(interval_ms_, std::bind(&CmdProcessor::timer_callback, this)); 75 | 76 | //Publishers 77 | pub_high_cmd_ = create_publisher("high_cmd", 10); 78 | 79 | //Subscribers 80 | sub_cmd_vel_ = create_subscription( 81 | "cmd_vel", 82 | 10, 83 | std::bind(&CmdProcessor::cmd_vel_callback, this, std::placeholders::_1) 84 | ); 85 | 86 | //Services 87 | srv_reset_state_ = create_service( 88 | "reset_state", 89 | std::bind(&CmdProcessor::reset_state_callback, this, 90 | std::placeholders::_1, std::placeholders::_2) 91 | ); 92 | srv_stand_up_ = create_service( 93 | "stand_up", 94 | std::bind(&CmdProcessor::stand_up_callback, this, 95 | std::placeholders::_1, std::placeholders::_2) 96 | ); 97 | srv_recover_stand_ = create_service( 98 | "recover_stand", 99 | std::bind(&CmdProcessor::recover_stand_callback, this, 100 | std::placeholders::_1, std::placeholders::_2) 101 | ); 102 | srv_lay_down_ = create_service( 103 | "lay_down", 104 | std::bind(&CmdProcessor::lay_down_callback, this, 105 | std::placeholders::_1, std::placeholders::_2) 106 | ); 107 | srv_damping_ = create_service( 108 | "damping", 109 | std::bind(&CmdProcessor::damping_callback, this, 110 | std::placeholders::_1, std::placeholders::_2) 111 | ); 112 | srv_jump_yaw_ = create_service( 113 | "jump_yaw", 114 | std::bind(&CmdProcessor::jump_yaw_callback, this, 115 | std::placeholders::_1, std::placeholders::_2) 116 | ); 117 | srv_beg_ = create_service( 118 | "beg", 119 | std::bind(&CmdProcessor::beg_callback, this, 120 | std::placeholders::_1, std::placeholders::_2) 121 | ); 122 | srv_dance1_ = create_service( 123 | "dance1", 124 | std::bind(&CmdProcessor::dance1_callback, this, 125 | std::placeholders::_1, std::placeholders::_2) 126 | ); 127 | srv_dance2_ = create_service( 128 | "dance2", 129 | std::bind(&CmdProcessor::dance2_callback, this, 130 | std::placeholders::_1, std::placeholders::_2) 131 | ); 132 | srv_set_body_rpy_ = create_service( 133 | "set_body_rpy", 134 | std::bind(&CmdProcessor::set_body_rpy_callback, this, 135 | std::placeholders::_1, std::placeholders::_2) 136 | ); 137 | srv_set_gait_ = create_service( 138 | "set_gait", 139 | std::bind(&CmdProcessor::set_gait_callback, this, 140 | std::placeholders::_1, std::placeholders::_2) 141 | ); 142 | 143 | 144 | //Misc variables 145 | cmd_vel_counter_ = cmd_vel_timeout_; // init to timeout value so we don't publish 146 | 147 | //Default command values 148 | high_cmd_.head[0] = 0xFE; //?? 149 | high_cmd_.head[1] = 0xEF; //?? 150 | high_cmd_.level_flag = 0xEE; //from unitree_legged_sdk, HIGHLEVEL TODO - move to UDP node? 151 | high_cmd_.mode = to_value(Go1Mode::idle); 152 | high_cmd_.gait_type = to_value(Go1Gait::idle); 153 | high_cmd_.speed_level = to_value(Go1SpeedLevel::low); 154 | high_cmd_.velocity[0] = 0.0; 155 | high_cmd_.velocity[1] = 0.0; 156 | high_cmd_.yaw_speed = 0.0; 157 | 158 | // default walking gait is trotting 159 | walking_gait_ = Go1Gait::trot; 160 | 161 | RCLCPP_INFO_STREAM(get_logger(), "cmd_processor node started"); 162 | } 163 | 164 | private: 165 | rclcpp::TimerBase::SharedPtr timer_; 166 | rclcpp::Publisher::SharedPtr pub_high_cmd_; 167 | rclcpp::Subscription::SharedPtr sub_cmd_vel_; 168 | rclcpp::Service::SharedPtr srv_reset_state_; 169 | rclcpp::Service::SharedPtr srv_stand_up_; 170 | rclcpp::Service::SharedPtr srv_recover_stand_; 171 | rclcpp::Service::SharedPtr srv_lay_down_; 172 | rclcpp::Service::SharedPtr srv_damping_; 173 | rclcpp::Service::SharedPtr srv_jump_yaw_; 174 | rclcpp::Service::SharedPtr srv_beg_; 175 | rclcpp::Service::SharedPtr srv_dance1_; 176 | rclcpp::Service::SharedPtr srv_dance2_; 177 | rclcpp::Service::SharedPtr srv_set_body_rpy_; 178 | rclcpp::Service::SharedPtr srv_set_gait_; 179 | 180 | double rate_, interval_; 181 | std::chrono::milliseconds interval_ms_, cmd_vel_timeout_, cmd_vel_counter_; 182 | geometry_msgs::msg::Twist cmd_vel_ {}; 183 | ros2_unitree_legged_msgs::msg::HighCmd high_cmd_ {}; 184 | bool reset_state_ = false; 185 | uint8_t reset_counter_ = 0; 186 | Go1Gait walking_gait_; 187 | 188 | void timer_callback() 189 | { 190 | 191 | //Only publish if we haven't timed out 192 | if (cmd_vel_counter_ < cmd_vel_timeout_) { 193 | //Increment counter by timer interval 194 | cmd_vel_counter_ += interval_ms_; 195 | 196 | } else { //cmd_vel has timed out 197 | //reset to 0 twist 198 | reset_cmd_vel(); 199 | } 200 | 201 | //Build high command 202 | high_cmd_.velocity[0] = cmd_vel_.linear.x; 203 | high_cmd_.velocity[1] = cmd_vel_.linear.y; 204 | high_cmd_.yaw_speed = cmd_vel_.angular.z; 205 | 206 | //publish high command 207 | pub_high_cmd_->publish(high_cmd_); 208 | 209 | //Reset state to idle after one message is sent for anything but walking 210 | if (reset_state_) { 211 | if (reset_counter_ >= 10) { 212 | high_cmd_.mode = to_value(Go1Mode::idle); 213 | reset_state_ = false; 214 | } 215 | reset_counter_++; 216 | } 217 | } 218 | 219 | void reset_state() { 220 | reset_state_ = true; 221 | reset_counter_ = 0; 222 | } 223 | 224 | void cmd_vel_callback(const geometry_msgs::msg::Twist & msg) 225 | { 226 | //Store received cmd_vel 227 | cmd_vel_ = msg; 228 | 229 | //reset timeout 230 | cmd_vel_counter_ = 0ms; 231 | 232 | //set mode and gait type 233 | high_cmd_.mode = to_value(Go1Mode::target_velocity_walking); 234 | high_cmd_.gait_type = to_value(walking_gait_); 235 | } 236 | 237 | void reset_cmd_vel() { 238 | cmd_vel_.linear.x = 0.0; 239 | cmd_vel_.linear.y = 0.0; 240 | cmd_vel_.linear.z = 0.0; 241 | cmd_vel_.angular.x = 0.0; 242 | cmd_vel_.angular.y = 0.0; 243 | cmd_vel_.angular.z = 0.0; 244 | } 245 | 246 | //put the dog back into idle state 247 | void reset_state_callback( 248 | const std::shared_ptr, 249 | std::shared_ptr 250 | ) { 251 | reset_cmd_vel(); 252 | reset_state(); //Reset state after command is sent 253 | } 254 | 255 | //put the dog in the stand up state 256 | void stand_up_callback( 257 | const std::shared_ptr, 258 | std::shared_ptr 259 | ) { 260 | reset_cmd_vel(); 261 | high_cmd_.mode = to_value(Go1Mode::position_stand_up); 262 | reset_state(); //Reset state after command is sent 263 | } 264 | 265 | //put the dog in the recover stand state 266 | void recover_stand_callback( 267 | const std::shared_ptr, 268 | std::shared_ptr 269 | ) { 270 | reset_cmd_vel(); 271 | high_cmd_.mode = to_value(Go1Mode::recovery_stand); 272 | reset_state(); //Reset state after command is sent 273 | } 274 | 275 | //To lay down completely, call lay down service and then damping service 276 | 277 | //put the dog in the stand down state 278 | void lay_down_callback( 279 | const std::shared_ptr, 280 | std::shared_ptr 281 | ) { 282 | reset_cmd_vel(); 283 | high_cmd_.mode = to_value(Go1Mode::position_stand_down); 284 | reset_state(); //Reset state after command is sent 285 | } 286 | 287 | //put the dog in the damping state 288 | // DO NOT CALL WHILE DOG IS IN STANDING STATE 289 | void damping_callback( 290 | const std::shared_ptr, 291 | std::shared_ptr 292 | ) { 293 | reset_cmd_vel(); 294 | high_cmd_.mode = to_value(Go1Mode::damping); 295 | reset_state(); //Reset state after command is sent 296 | } 297 | 298 | //put the dog into jump_yaw state 299 | void jump_yaw_callback( 300 | const std::shared_ptr, 301 | std::shared_ptr 302 | ) { 303 | reset_cmd_vel(); 304 | high_cmd_.mode = to_value(Go1Mode::jump_yaw); 305 | reset_state(); //Reset state after command is sent 306 | } 307 | 308 | //put the dog into straight_hand state 309 | void beg_callback( 310 | const std::shared_ptr, 311 | std::shared_ptr 312 | ) { 313 | reset_cmd_vel(); 314 | high_cmd_.mode = to_value(Go1Mode::straight_hand); 315 | reset_state(); //Reset state after command is sent 316 | } 317 | 318 | //put the dog into dance1 state 319 | void dance1_callback( 320 | const std::shared_ptr, 321 | std::shared_ptr 322 | ) { 323 | reset_cmd_vel(); 324 | high_cmd_.mode = to_value(Go1Mode::dance1); 325 | reset_state(); //Reset state after command is sent 326 | } 327 | 328 | //put the dog into dance2 state 329 | void dance2_callback( 330 | const std::shared_ptr, 331 | std::shared_ptr 332 | ) { 333 | reset_cmd_vel(); 334 | high_cmd_.mode = to_value(Go1Mode::dance2); 335 | reset_state(); //Reset state after command is sent 336 | } 337 | 338 | void set_body_rpy_callback( 339 | const std::shared_ptr request, 340 | std::shared_ptr 341 | ) { 342 | reset_cmd_vel(); 343 | 344 | high_cmd_.euler[0] = request->roll; 345 | high_cmd_.euler[1] = request->pitch; 346 | high_cmd_.euler[2] = request->yaw; 347 | 348 | high_cmd_.mode = to_value(Go1Mode::force_stand); 349 | } 350 | 351 | void set_gait_callback( 352 | const std::shared_ptr request, 353 | std::shared_ptr 354 | ) { 355 | // check that we are not setting an invalid gait type 356 | if (request->gait <= 4){ 357 | walking_gait_ = static_cast(request->gait); 358 | } 359 | } 360 | 361 | }; 362 | 363 | int main(int argc, char ** argv) 364 | { 365 | rclcpp::init(argc, argv); 366 | rclcpp::spin(std::make_shared()); 367 | rclcpp::shutdown(); 368 | return 0; 369 | } -------------------------------------------------------------------------------- /unitree_nav/src/nav_to_pose.cpp: -------------------------------------------------------------------------------- 1 | //This node is an example for working with the Nav2 stack to command 2 | //the Unitree Go1 to a certain pose in the map. 3 | 4 | #include 5 | #include 6 | #include 7 | #include "rclcpp/rclcpp.hpp" 8 | #include "rclcpp_action/rclcpp_action.hpp" 9 | #include "std_srvs/srv/empty.hpp" 10 | #include "geometry_msgs/msg/quaternion.hpp" 11 | #include "tf2/LinearMath/Quaternion.h" 12 | #include "tf2/LinearMath/Matrix3x3.h" 13 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 14 | #include "nav2_msgs/action/navigate_to_pose.hpp" 15 | #include "unitree_nav_interfaces/srv/nav_to_pose.hpp" 16 | 17 | //https://stackoverflow.com/questions/11714325/how-to-get-enum-item-name-from-its-value 18 | #define STATES \ 19 | X(IDLE, "IDLE") \ 20 | X(SEND_GOAL, "SEND_GOAL") \ 21 | X(WAIT_FOR_GOAL_RESPONSE, "WAIT_FOR_GOAL_RESPONSE") \ 22 | X(WAIT_FOR_MOVEMENT_COMPLETE, "WAIT_FOR_MOVEMENT_COMPLETE") 23 | 24 | #define X(state, name) state, 25 | enum class State : size_t {STATES}; 26 | #undef X 27 | 28 | #define X(state, name) name, 29 | std::vector STATE_NAMES = {STATES}; 30 | #undef X 31 | 32 | //https://stackoverflow.com/questions/11421432/how-can-i-output-the-value-of-an-enum-class-in-c11 33 | template 34 | auto to_value(Enumeration const value) 35 | -> typename std::underlying_type::type 36 | { 37 | return static_cast::type>(value); 38 | } 39 | 40 | auto get_state_name(State state) { 41 | return STATE_NAMES[to_value(state)]; 42 | } 43 | 44 | std::tuple quaternion_to_rpy(const geometry_msgs::msg::Quaternion & q); 45 | geometry_msgs::msg::Quaternion rpy_to_quaternion(double roll, double pitch, double yaw); 46 | 47 | using namespace std::chrono_literals; 48 | 49 | class NavToPose : public rclcpp::Node 50 | { 51 | public: 52 | NavToPose() 53 | : Node("nav_to_pose") 54 | { 55 | 56 | //Parameters 57 | auto param = rcl_interfaces::msg::ParameterDescriptor{}; 58 | param.description = "The frame in which poses are sent."; 59 | declare_parameter("pose_frame", "map", param); 60 | goal_msg_.pose.header.frame_id = get_parameter("pose_frame").get_parameter_value().get(); 61 | 62 | //Timers 63 | timer_ = create_wall_timer( 64 | static_cast(static_cast(interval_ * 1000.0)), 65 | std::bind(&NavToPose::timer_callback, this) 66 | ); 67 | 68 | //Services 69 | srv_nav_to_pose_ = create_service( 70 | "unitree_nav_to_pose", 71 | std::bind(&NavToPose::srv_nav_to_pose_callback, this, 72 | std::placeholders::_1, std::placeholders::_2) 73 | ); 74 | 75 | srv_cancel_nav_ = create_service( 76 | "unitree_cancel_nav", 77 | std::bind(&NavToPose::cancel_nav_callback, this, 78 | std::placeholders::_1, std::placeholders::_2) 79 | ); 80 | 81 | //Action Clients 82 | act_nav_to_pose_ = rclcpp_action::create_client( 83 | this, 84 | "navigate_to_pose" 85 | ); 86 | 87 | RCLCPP_INFO_STREAM(get_logger(), "nav_to_pose node started"); 88 | } 89 | 90 | private: 91 | rclcpp::TimerBase::SharedPtr timer_; 92 | rclcpp::Service::SharedPtr srv_nav_to_pose_; 93 | rclcpp::Service::SharedPtr srv_cancel_nav_; 94 | rclcpp_action::Client::SharedPtr act_nav_to_pose_; 95 | 96 | double rate_ = 100.0; //Hz 97 | double interval_ = 1.0 / rate_; //seconds 98 | State state_ = State::IDLE; 99 | State state_last_ = state_; 100 | State state_next_ = state_; 101 | rclcpp_action::Client::Goal goal_msg_ {}; 102 | bool goal_response_received_ = false; 103 | rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_ {}; 104 | std::shared_ptr feedback_ = nullptr; 105 | std::shared_ptr::WrappedResult> 106 | result_ = nullptr; 107 | 108 | 109 | void timer_callback() 110 | { 111 | 112 | state_ = state_next_; 113 | 114 | auto new_state = state_ != state_last_; 115 | 116 | if (new_state) { 117 | RCLCPP_INFO_STREAM(get_logger(), "nav_to_pose state changed to " << get_state_name(state_)); 118 | 119 | state_last_ = state_; 120 | } 121 | 122 | switch(state_) { 123 | case State::IDLE: 124 | { 125 | break; 126 | } 127 | case State::SEND_GOAL: 128 | { 129 | if(act_nav_to_pose_->wait_for_action_server(0s)) { 130 | //Reset status flags and pointers 131 | goal_response_received_ = false; 132 | goal_handle_ = rclcpp_action::ClientGoalHandle::SharedPtr {}; 133 | result_ = nullptr; 134 | 135 | //Construct and send goal 136 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 137 | send_goal_options.goal_response_callback = 138 | std::bind(&NavToPose::goal_response_callback, this, std::placeholders::_1); 139 | send_goal_options.feedback_callback = 140 | std::bind(&NavToPose::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); 141 | send_goal_options.result_callback = 142 | std::bind(&NavToPose::result_callback, this, std::placeholders::_1); 143 | act_nav_to_pose_->async_send_goal(goal_msg_, send_goal_options); 144 | 145 | state_next_ = State::WAIT_FOR_GOAL_RESPONSE; 146 | } else { 147 | RCLCPP_ERROR_STREAM(get_logger(), "Action server not available, aborting."); 148 | state_next_ = State::IDLE; 149 | } 150 | 151 | break; 152 | } 153 | case State::WAIT_FOR_GOAL_RESPONSE: 154 | { 155 | //TODO add timeout 156 | if (goal_response_received_) { 157 | if (goal_handle_) { 158 | RCLCPP_INFO(get_logger(), "Goal accepted by server, waiting for result"); 159 | state_next_ = State::WAIT_FOR_MOVEMENT_COMPLETE; 160 | } else { 161 | RCLCPP_ERROR_STREAM(get_logger(), "Goal was rejected by server"); 162 | state_next_ = State::IDLE; 163 | } 164 | } 165 | 166 | break; 167 | } 168 | case State::WAIT_FOR_MOVEMENT_COMPLETE: 169 | { 170 | if (result_) { 171 | state_next_ = State::IDLE; 172 | } 173 | break; 174 | } 175 | default: 176 | auto msg = "Unhandled state: " + get_state_name(state_); 177 | RCLCPP_ERROR_STREAM(get_logger(), msg); 178 | throw std::logic_error(msg); 179 | break; 180 | } 181 | 182 | } 183 | 184 | void srv_nav_to_pose_callback( 185 | const std::shared_ptr request, 186 | std::shared_ptr 187 | ) { 188 | //Store requested pose 189 | goal_msg_.pose.pose.position.x = request->x; 190 | goal_msg_.pose.pose.position.y = request->y; 191 | goal_msg_.pose.pose.orientation = rpy_to_quaternion(0.0, 0.0, request->theta); 192 | 193 | //Initiate action call 194 | state_next_ = State::SEND_GOAL; 195 | } 196 | 197 | void cancel_nav_callback( 198 | const std::shared_ptr, 199 | std::shared_ptr 200 | ) { 201 | RCLCPP_INFO_STREAM(get_logger(), "Cancelling navigation."); 202 | act_nav_to_pose_->async_cancel_all_goals(); 203 | state_next_ = State::IDLE; 204 | } 205 | 206 | void goal_response_callback( 207 | const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle 208 | ) { 209 | goal_response_received_ = true; 210 | goal_handle_ = goal_handle; 211 | RCLCPP_INFO_STREAM(get_logger(), "Goal response"); 212 | } 213 | 214 | void feedback_callback( 215 | rclcpp_action::ClientGoalHandle::SharedPtr, 216 | const std::shared_ptr feedback 217 | ) { 218 | //Store result for later use 219 | feedback_ = feedback; 220 | 221 | if (feedback_) { 222 | auto [roll, pitch, yaw] = quaternion_to_rpy(feedback_->current_pose.pose.orientation); 223 | 224 | RCLCPP_INFO_STREAM(get_logger(), "x = " << feedback_->current_pose.pose.position.x 225 | << ", y = " << feedback_->current_pose.pose.position.y 226 | << ", theta = " << yaw 227 | ); 228 | } 229 | } 230 | 231 | void result_callback( 232 | const rclcpp_action::ClientGoalHandle::WrappedResult & result 233 | ) { 234 | switch (result.code) { 235 | case rclcpp_action::ResultCode::SUCCEEDED: 236 | RCLCPP_INFO(this->get_logger(), "Goal succeeded"); 237 | break; 238 | case rclcpp_action::ResultCode::ABORTED: 239 | RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); 240 | return; 241 | case rclcpp_action::ResultCode::CANCELED: 242 | RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); 243 | return; 244 | default: 245 | RCLCPP_ERROR(this->get_logger(), "Unknown result code"); 246 | return; 247 | } 248 | 249 | //Store result for later use 250 | result_ = std::make_shared::WrappedResult>(); 251 | *result_ = result; 252 | } 253 | }; 254 | 255 | std::tuple quaternion_to_rpy(const geometry_msgs::msg::Quaternion & q) { 256 | //https://answers.ros.org/question/339528/quaternion-to-rpy-ros2/ 257 | tf2::Quaternion q_temp; 258 | tf2::fromMsg(q, q_temp); 259 | tf2::Matrix3x3 m(q_temp); 260 | double roll, pitch, yaw; 261 | m.getRPY(roll, pitch, yaw); 262 | return {roll, pitch, yaw}; 263 | } 264 | geometry_msgs::msg::Quaternion rpy_to_quaternion(double roll, double pitch, double yaw) { 265 | tf2::Quaternion q; 266 | q.setRPY(roll, pitch, yaw); 267 | return tf2::toMsg(q); 268 | } 269 | 270 | int main(int argc, char ** argv) 271 | { 272 | rclcpp::init(argc, argv); 273 | rclcpp::spin(std::make_shared()); 274 | rclcpp::shutdown(); 275 | return 0; 276 | } -------------------------------------------------------------------------------- /unitree_nav_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(unitree_nav_interfaces) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(geometry_msgs REQUIRED) 11 | find_package(rosidl_default_generators REQUIRED) 12 | ament_export_dependencies(rosidl_default_runtime) 13 | 14 | rosidl_generate_interfaces( 15 | ${PROJECT_NAME}_srv 16 | "srv/SetBodyRPY.srv" 17 | "srv/NavToPose.srv" 18 | "srv/SetGait.srv" 19 | LIBRARY_NAME ${PROJECT_NAME} 20 | DEPENDENCIES 21 | geometry_msgs 22 | ) 23 | 24 | if(BUILD_TESTING) 25 | find_package(ament_lint_auto REQUIRED) 26 | # the following line skips the linter which checks for copyrights 27 | # comment the line when a copyright and license is added to all source files 28 | set(ament_cmake_copyright_FOUND TRUE) 29 | # the following line skips cpplint (only works in a git repo) 30 | # comment the line when this package is in a git repo and when 31 | # a copyright and license is added to all source files 32 | set(ament_cmake_cpplint_FOUND TRUE) 33 | ament_lint_auto_find_test_dependencies() 34 | endif() 35 | 36 | ament_package() 37 | -------------------------------------------------------------------------------- /unitree_nav_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | unitree_nav_interfaces 5 | 0.0.0 6 | Navigation control interfaces for Unitree Go1 7 | Nick Morales 8 | Apache License 2.0 9 | 10 | ament_lint_auto 11 | ament_lint_common 12 | geometry_msgs 13 | rosidl_default_generators 14 | rosidl_default_runtime 15 | 16 | ament_cmake 17 | 18 | rosidl_interface_packages 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /unitree_nav_interfaces/srv/NavToPose.srv: -------------------------------------------------------------------------------- 1 | float64 x # x coordinate of goal pose 2 | float64 y # y coordinate of goal pose 3 | float64 theta # rotation of goal pose 4 | --- -------------------------------------------------------------------------------- /unitree_nav_interfaces/srv/SetBodyRPY.srv: -------------------------------------------------------------------------------- 1 | float64 roll # rotation about the x-axis 2 | # range is about -0.8 to 0.8 3 | float64 pitch # rotation about the y-axis 4 | # negative to look up, positive to look down 5 | # range is about -0.6 to 0.6 6 | float64 yaw # rotation about the z-axis 7 | # negative to look left, positive to look right 8 | # range is about -0.3 to 0.3 9 | --- -------------------------------------------------------------------------------- /unitree_nav_interfaces/srv/SetGait.srv: -------------------------------------------------------------------------------- 1 | uint8 IDLE=0 2 | uint8 TROT=1 3 | uint8 TROT_RUNNING=2 4 | uint8 CLIMB_STAIRS=3 5 | uint8 TROT_OBSTACLE=4 6 | 7 | uint8 gait 8 | --- -------------------------------------------------------------------------------- /unitree_nav_launch_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | unitree_nav_launch_module 5 | 0.0.0 6 | Launch module library for the unitree_nav package. 7 | Nick Morales 8 | Apache License 2.0 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /unitree_nav_launch_module/resource/unitree_nav_launch_module: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ngmor/unitree_nav/2f633b806600bf09a48bbca063421fd0eb3f9a7b/unitree_nav_launch_module/resource/unitree_nav_launch_module -------------------------------------------------------------------------------- /unitree_nav_launch_module/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/unitree_nav_launch_module 3 | [install] 4 | install_scripts=$base/lib/unitree_nav_launch_module 5 | -------------------------------------------------------------------------------- /unitree_nav_launch_module/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'unitree_nav_launch_module' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='Nick Morales', 17 | maintainer_email='ngmorales97@gmail.com', 18 | description='Launch module library for the unitree_nav package.', 19 | license='Apache License 2.0', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | ], 24 | }, 25 | ) 26 | -------------------------------------------------------------------------------- /unitree_nav_launch_module/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /unitree_nav_launch_module/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /unitree_nav_launch_module/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /unitree_nav_launch_module/unitree_nav_launch_module/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Nick Morales. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | """Custom package for unitree_nav launch files.""" 16 | 17 | 18 | from .ternary_text_substitution import TernaryTextSubstitution 19 | 20 | __all__ = [ 21 | 'TernaryTextSubstitution', 22 | ] -------------------------------------------------------------------------------- /unitree_nav_launch_module/unitree_nav_launch_module/ternary_text_substitution.py: -------------------------------------------------------------------------------- 1 | from typing import Text 2 | 3 | from launch.condition import Condition 4 | from launch.substitution import Substitution 5 | from launch.launch_context import LaunchContext 6 | 7 | class TernaryTextSubstitution(Substitution): 8 | """Substitution that returns the first text argument if the condition is true, otherwise it returns the second text argument.""" 9 | def __init__(self, condition: Condition, true_text: Text, false_text: Text) -> None: 10 | """Create a TernaryTextSubstitution.""" 11 | super().__init__() 12 | 13 | if not isinstance(true_text, Text): 14 | raise TypeError( 15 | "TernaryTextSubstitution expected Text object got '{}' instead.".format(type(true_text)) 16 | ) 17 | if not isinstance(false_text, Text): 18 | raise TypeError( 19 | "TernaryTextSubstitution expected Text object got '{}' instead.".format(type(false_text)) 20 | ) 21 | 22 | if not isinstance(condition, Condition): 23 | raise TypeError( 24 | "TernaryTextSubstitution expected Condition object got '{}' instead.".format(type(condition)) 25 | ) 26 | 27 | self.__true_text = true_text 28 | self.__false_text = false_text 29 | self.__condition = condition 30 | 31 | @property 32 | def true_text(self) -> Text: 33 | """Getter for true text.""" 34 | return self.__true_text 35 | 36 | @property 37 | def false_text(self) -> Text: 38 | """Getter for false text.""" 39 | return self.__false_text 40 | 41 | def describe(self) -> Text: 42 | """Return a description of this substitution as a string.""" 43 | return f"'{self.true_text}' if true, '{self.false_text}' if false" 44 | 45 | def perform(self, context: LaunchContext) -> Text: 46 | """Perform the substitution by returning the string itself.""" 47 | if self.__condition.evaluate(context): 48 | return self.true_text 49 | else: 50 | return self.false_text --------------------------------------------------------------------------------