├── .vscode ├── c_cpp_properties.json └── settings.json ├── CMakeLists.txt ├── README.md ├── config ├── a1_dev_path.rviz ├── vilo_rviz_config.rviz └── vins_rviz_config.rviz ├── doc └── images │ └── a1_visualizer.png ├── launch ├── isaac_a1_dev.launch ├── isaac_a1_vilo.launch ├── isaac_a1_vins.launch ├── vilo_rviz.launch └── vins_rviz.launch ├── package.xml └── src ├── .vscode ├── c_cpp_properties.json └── settings.json ├── hardware_a1_visualize.cpp ├── isaac_a1_visualize.cpp └── robot_markers ├── builder.cpp ├── builder.h ├── forward_kinematics.cpp └── forward_kinematics.h /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "", 6 | "limitSymbolsToIncludedHeaders": true 7 | }, 8 | "includePath": [ 9 | "/home/shuoy/nv_work/util_ws/devel/include/**", 10 | "/home/shuoy/nv_work/unitree_ws/devel/include/**", 11 | "/home/shuoy/nv_work/ros_catkin_ws/install/include/**", 12 | "/home/shuoy/nv_work/ros_catkin_ws/devel/include/**", 13 | "/home/shuoy/nv_work/unitree_ws/src/VINS-Fusion/camera_models/include/**", 14 | "/home/shuoy/nv_work/unitree_ws/src/elevation_mapping/elevation_mapping/include/**", 15 | "/home/shuoy/nv_work/unitree_ws/src/elevation_mapping/elevation_mapping_demos/include/**", 16 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_core/include/**", 17 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_costmap_2d/include/**", 18 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_cv/include/**", 19 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_demos/include/**", 20 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_filters/include/**", 21 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_loader/include/**", 22 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_octomap/include/**", 23 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_pcl/include/**", 24 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_ros/include/**", 25 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_rviz_plugin/include/**", 26 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_sdf/include/**", 27 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_visualization/include/**", 28 | "/home/shuoy/nv_work/unitree_ws/src/kindr/include/**", 29 | "/home/shuoy/nv_work/unitree_ws/src/kindr_ros/kindr_ros/include/**", 30 | "/home/shuoy/nv_work/unitree_ws/src/kindr_ros/kindr_rviz_plugins/include/**", 31 | "/home/shuoy/nv_work/unitree_ws/src/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/**", 32 | "/home/shuoy/nv_work/util_ws/src/PlotJuggler/include/**", 33 | "/home/shuoy/nv_work/unitree_ws/src/unitree_ros/unitree_controller/include/**", 34 | "/home/shuoy/nv_work/unitree_ws/src/unitree_ros/unitree_legged_control/include/**", 35 | "/home/shuoy/nv_work/unitree_ws/src/unitree_ros/unitree_legged_real/include/**", 36 | "/usr/include/**" 37 | ], 38 | "name": "ROS" 39 | } 40 | ], 41 | "version": 4 42 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/shuoy/nv_work/util_ws/devel/lib/python3/dist-packages", 4 | "/home/shuoy/nv_work/unitree_ws/devel/lib/python3/dist-packages", 5 | "/home/shuoy/nv_work/ros_catkin_ws/install/lib/python3/dist-packages", 6 | "/home/shuoy/nv_work/ros_catkin_ws/devel/lib/python3/dist-packages", 7 | "/home/shuoy/nv_work/carbgym/python", 8 | "/home/shuoy/nv_work/laikago_rl" 9 | ], 10 | "files.associations": { 11 | "unordered_map": "cpp", 12 | "unordered_set": "cpp" 13 | }, 14 | "python.analysis.extraPaths": [ 15 | "/home/shuoy/nv_work/util_ws/devel/lib/python3/dist-packages", 16 | "/home/shuoy/nv_work/unitree_ws/devel/lib/python3/dist-packages", 17 | "/home/shuoy/nv_work/ros_catkin_ws/install/lib/python3/dist-packages", 18 | "/home/shuoy/nv_work/ros_catkin_ws/devel/lib/python3/dist-packages", 19 | "/home/shuoy/nv_work/carbgym/python", 20 | "/home/shuoy/nv_work/laikago_rl" 21 | ] 22 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(a1_visualizer) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | SET (CMAKE_CXX_STANDARD 11) 7 | SET (CMAKE_CXX_STANDARD_REQUIRED ON) 8 | SET (CMAKE_CXX_FLAGS "-O3") 9 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive") 10 | 11 | 12 | # added to supprt robot_markers, this one should be included with ros-version-orocos-kdl 13 | find_package(orocos_kdl REQUIRED) 14 | 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | roscpp 18 | rospy 19 | roslib 20 | rosbag 21 | geometry_msgs 22 | message_generation 23 | 24 | 25 | # added to support robot_markers 26 | kdl_parser 27 | tf2_kdl 28 | tf2_ros 29 | transform_graph 30 | urdf 31 | visualization_msgs 32 | 33 | 34 | # unitree_legged_msgs 35 | ) 36 | 37 | # DEPENDS system_lib 38 | catkin_package( 39 | # INCLUDE_DIRS include 40 | # LIBRARIES 41 | CATKIN_DEPENDS 42 | roscpp 43 | rospy 44 | roslib 45 | std_msgs 46 | geometry_msgs 47 | rosbag 48 | message_runtime 49 | ) 50 | 51 | 52 | include_directories( 53 | ${CMAKE_CURRENT_SOURCE_DIR}/src 54 | ${catkin_INCLUDE_DIRS} 55 | # added to supprt robot_markers 56 | ${orocos_kdl_INCLUDE_DIRS} 57 | ) 58 | 59 | 60 | SET(A1_SOURCES 61 | ${CMAKE_CURRENT_SOURCE_DIR}/src/hardware_a1_visualize.cpp 62 | ${CMAKE_CURRENT_SOURCE_DIR}/src/robot_markers/builder.cpp 63 | ${CMAKE_CURRENT_SOURCE_DIR}/src/robot_markers/forward_kinematics.cpp 64 | ) 65 | 66 | add_executable(hardware_a1_visualizer 67 | ${A1_SOURCES} ) 68 | target_link_libraries( hardware_a1_visualizer 69 | ${orocos_kdl_LIBRARIES} 70 | ${catkin_LIBRARIES} 71 | ) 72 | 73 | 74 | # this is for Isaac Sim robot 75 | SET(ISAAC_A1_SOURCES 76 | ${CMAKE_CURRENT_SOURCE_DIR}/src/isaac_a1_visualize.cpp 77 | ${CMAKE_CURRENT_SOURCE_DIR}/src/robot_markers/builder.cpp 78 | ${CMAKE_CURRENT_SOURCE_DIR}/src/robot_markers/forward_kinematics.cpp 79 | ) 80 | 81 | add_executable(isaac_a1_visualizer 82 | ${ISAAC_A1_SOURCES} ) 83 | target_link_libraries( isaac_a1_visualizer 84 | ${orocos_kdl_LIBRARIES} 85 | ${catkin_LIBRARIES} ) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | A1_visualizer 2 | 3 | 4 | This visualizer visulize your A1 robot in rviz 5 | 6 | 7 | 8 | Dependency: 9 | 10 | Other than ROS itself (only tested in ROS melodic) 11 | 12 | 1. sudo apt install libpcl-dev 13 | 2. tranform graph https://github.com/jstnhuang/transform_graph 14 | 3. sudo apt-get install ros-melodic-orocos-kdl 15 | 4. Unitree ROS https://github.com/paulyang1990/unitree_ros 16 | 17 | Follow the README carefully, and remember the unitree_legged_sdk version must be v3.2. Refer to A1-Docker [dockerfile](https://github.com/paulyang1990/A1-Docker/blob/main/docker/Dockerfile) for useful commands 18 | 19 | 20 | 21 | The visualizer will search for unitree ros package installed on your machine, then read urdf from unitree ros package. 22 | 23 | Please make sure you can roscd unitree_legged_real before running this package. 24 | 25 | 26 | 27 | The visualization result looks like this: 28 | ![visualize_image](doc/images/a1_visualizer.png) 29 | 30 | (The camera frame and points in the scene comes from VINS-Fusion. They are not part of this package) -------------------------------------------------------------------------------- /config/a1_dev_path.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /VIOGroup1/VIOPath1 9 | - /VIOGroup1/CameraMarker1 10 | - /VIOGroup1/PointCloud1 11 | - /VIOGroup1/HistoryPointCloud1 12 | - /VIOGroup1/track_image1 13 | - /GlobalGroup1/CarModel1 14 | - /TF1/Frames1 15 | - /foot_paths1 16 | Splitter Ratio: 0.4651159942150116 17 | Tree Height: 371 18 | - Class: rviz/Selection 19 | Name: Selection 20 | - Class: rviz/Tool Properties 21 | Expanded: 22 | - /2D Pose Estimate1 23 | - /2D Nav Goal1 24 | - /Publish Point1 25 | Name: Tool Properties 26 | Splitter Ratio: 0.5886790156364441 27 | - Class: rviz/Views 28 | Expanded: 29 | - /Current View1 30 | Name: Views 31 | Splitter Ratio: 0.5 32 | - Class: rviz/Time 33 | Experimental: false 34 | Name: Time 35 | SyncMode: 0 36 | SyncSource: "" 37 | - Class: rviz/Displays 38 | Help Height: 78 39 | Name: Displays 40 | Property Tree Widget: 41 | Expanded: ~ 42 | Splitter Ratio: 0.5 43 | Tree Height: 363 44 | Preferences: 45 | PromptSaveOnExit: true 46 | Toolbars: 47 | toolButtonStyle: 2 48 | Visualization Manager: 49 | Class: "" 50 | Displays: 51 | - Alpha: 0.5 52 | Cell Size: 1 53 | Class: rviz/Grid 54 | Color: 160; 160; 164 55 | Enabled: true 56 | Line Style: 57 | Line Width: 0.029999999329447746 58 | Value: Lines 59 | Name: Grid 60 | Normal Cell Count: 0 61 | Offset: 62 | X: 0 63 | Y: 0 64 | Z: 0 65 | Plane: XY 66 | Plane Cell Count: 10 67 | Reference Frame: 68 | Value: true 69 | - Class: rviz/Axes 70 | Enabled: true 71 | Length: 0.5 72 | Name: Axes 73 | Radius: 0.05000000074505806 74 | Reference Frame: 75 | Value: true 76 | - Class: rviz/Group 77 | Displays: 78 | - Class: rviz/MarkerArray 79 | Enabled: true 80 | Marker Topic: /loop_fusion/pose_graph 81 | Name: loopLink 82 | Namespaces: 83 | {} 84 | Queue Size: 100 85 | Value: true 86 | - Class: rviz/MarkerArray 87 | Enabled: true 88 | Marker Topic: /loop_fusion/camera_pose_visual 89 | Name: loopCamera 90 | Namespaces: 91 | {} 92 | Queue Size: 100 93 | Value: true 94 | - Alpha: 1 95 | Buffer Length: 1 96 | Class: rviz/Path 97 | Color: 204; 0; 0 98 | Enabled: true 99 | Head Diameter: 0.30000001192092896 100 | Head Length: 0.20000000298023224 101 | Length: 0.30000001192092896 102 | Line Style: Lines 103 | Line Width: 0.03999999910593033 104 | Name: loopPath 105 | Offset: 106 | X: 0 107 | Y: 0 108 | Z: 0 109 | Pose Color: 255; 85; 255 110 | Pose Style: None 111 | Radius: 0.029999999329447746 112 | Shaft Diameter: 0.10000000149011612 113 | Shaft Length: 0.10000000149011612 114 | Topic: /loop_fusion/pose_graph_path 115 | Unreliable: false 116 | Value: true 117 | - Alpha: 1 118 | Autocompute Intensity Bounds: true 119 | Autocompute Value Bounds: 120 | Max Value: -0.00362956034950912 121 | Min Value: -0.15546569228172302 122 | Value: true 123 | Axis: Z 124 | Channel Name: intensity 125 | Class: rviz/PointCloud 126 | Color: 255; 255; 255 127 | Color Transformer: FlatColor 128 | Decay Time: 10 129 | Enabled: false 130 | Invert Rainbow: false 131 | Max Color: 255; 255; 255 132 | Max Intensity: 4096 133 | Min Color: 0; 0; 0 134 | Min Intensity: 0 135 | Name: PointCloud 136 | Position Transformer: XYZ 137 | Queue Size: 10 138 | Selectable: true 139 | Size (Pixels): 2 140 | Size (m): 0.009999999776482582 141 | Style: Points 142 | Topic: /loop_fusion/margin_cloud_loop_rect 143 | Unreliable: false 144 | Use Fixed Frame: true 145 | Use rainbow: true 146 | Value: false 147 | - Alpha: 1 148 | Autocompute Intensity Bounds: true 149 | Autocompute Value Bounds: 150 | Max Value: 10 151 | Min Value: -10 152 | Value: true 153 | Axis: Z 154 | Channel Name: intensity 155 | Class: rviz/PointCloud 156 | Color: 0; 255; 0 157 | Color Transformer: FlatColor 158 | Decay Time: 0 159 | Enabled: false 160 | Invert Rainbow: false 161 | Max Color: 255; 255; 255 162 | Max Intensity: 4096 163 | Min Color: 0; 0; 0 164 | Min Intensity: 0 165 | Name: PointCloud 166 | Position Transformer: XYZ 167 | Queue Size: 10 168 | Selectable: true 169 | Size (Pixels): 4 170 | Size (m): 0.009999999776482582 171 | Style: Points 172 | Topic: /loop_fusion/point_cloud_loop_rect 173 | Unreliable: false 174 | Use Fixed Frame: true 175 | Use rainbow: true 176 | Value: false 177 | - Class: rviz/Image 178 | Enabled: false 179 | Image Topic: /loop_fusion/match_image 180 | Max Value: 1 181 | Median window: 5 182 | Min Value: 0 183 | Name: loop_match_Image 184 | Normalize Range: true 185 | Queue Size: 2 186 | Transport Hint: raw 187 | Unreliable: false 188 | Value: false 189 | Enabled: true 190 | Name: LoopGroup 191 | - Class: rviz/Group 192 | Displays: 193 | - Alpha: 1 194 | Buffer Length: 1 195 | Class: rviz/Path 196 | Color: 25; 255; 0 197 | Enabled: true 198 | Head Diameter: 0.30000001192092896 199 | Head Length: 0.20000000298023224 200 | Length: 0.30000001192092896 201 | Line Style: Lines 202 | Line Width: 0.029999999329447746 203 | Name: VIOPath 204 | Offset: 205 | X: 0 206 | Y: 0 207 | Z: 0 208 | Pose Color: 255; 85; 255 209 | Pose Style: None 210 | Radius: 0.029999999329447746 211 | Shaft Diameter: 0.10000000149011612 212 | Shaft Length: 0.10000000149011612 213 | Topic: /vinsfusion/path 214 | Unreliable: false 215 | Value: true 216 | - Class: rviz/MarkerArray 217 | Enabled: true 218 | Marker Topic: /vinsfusion/camera_pose_visual 219 | Name: CameraMarker 220 | Namespaces: 221 | {} 222 | Queue Size: 100 223 | Value: true 224 | - Alpha: 1 225 | Autocompute Intensity Bounds: true 226 | Autocompute Value Bounds: 227 | Max Value: 10 228 | Min Value: -10 229 | Value: true 230 | Axis: Z 231 | Channel Name: intensity 232 | Class: rviz/PointCloud 233 | Color: 255; 255; 255 234 | Color Transformer: Intensity 235 | Decay Time: 0 236 | Enabled: true 237 | Invert Rainbow: false 238 | Max Color: 255; 255; 255 239 | Max Intensity: 4096 240 | Min Color: 0; 0; 0 241 | Min Intensity: 0 242 | Name: PointCloud 243 | Position Transformer: XYZ 244 | Queue Size: 10 245 | Selectable: true 246 | Size (Pixels): 2 247 | Size (m): 0.009999999776482582 248 | Style: Points 249 | Topic: /vileom/point_cloud 250 | Unreliable: false 251 | Use Fixed Frame: true 252 | Use rainbow: true 253 | Value: true 254 | - Alpha: 1 255 | Autocompute Intensity Bounds: true 256 | Autocompute Value Bounds: 257 | Max Value: 10 258 | Min Value: -10 259 | Value: true 260 | Axis: Z 261 | Channel Name: intensity 262 | Class: rviz/PointCloud 263 | Color: 0; 255; 0 264 | Color Transformer: FlatColor 265 | Decay Time: 100 266 | Enabled: true 267 | Invert Rainbow: false 268 | Max Color: 255; 255; 255 269 | Max Intensity: 4096 270 | Min Color: 0; 0; 0 271 | Min Intensity: 0 272 | Name: HistoryPointCloud 273 | Position Transformer: XYZ 274 | Queue Size: 10 275 | Selectable: true 276 | Size (Pixels): 1 277 | Size (m): 0.009999999776482582 278 | Style: Points 279 | Topic: /vileom/margin_cloud 280 | Unreliable: false 281 | Use Fixed Frame: true 282 | Use rainbow: true 283 | Value: true 284 | - Class: rviz/Image 285 | Enabled: false 286 | Image Topic: /vinsfusion/image_track 287 | Max Value: 1 288 | Median window: 5 289 | Min Value: 0 290 | Name: track_image 291 | Normalize Range: true 292 | Queue Size: 2 293 | Transport Hint: raw 294 | Unreliable: false 295 | Value: false 296 | Enabled: true 297 | Name: VIOGroup 298 | - Class: rviz/Group 299 | Displays: 300 | - Alpha: 1 301 | Buffer Length: 1 302 | Class: rviz/Path 303 | Color: 0; 0; 255 304 | Enabled: true 305 | Head Diameter: 0.30000001192092896 306 | Head Length: 0.20000000298023224 307 | Length: 0.30000001192092896 308 | Line Style: Lines 309 | Line Width: 0.029999999329447746 310 | Name: GloablPath 311 | Offset: 312 | X: 0 313 | Y: 0 314 | Z: 0 315 | Pose Color: 255; 85; 255 316 | Pose Style: None 317 | Radius: 0.029999999329447746 318 | Shaft Diameter: 0.10000000149011612 319 | Shaft Length: 0.10000000149011612 320 | Topic: /globalEstimator/global_path 321 | Unreliable: false 322 | Value: true 323 | - Class: rviz/MarkerArray 324 | Enabled: true 325 | Marker Topic: /globalEstimator/car_model 326 | Name: CarModel 327 | Namespaces: 328 | {} 329 | Queue Size: 100 330 | Value: true 331 | Enabled: true 332 | Name: GlobalGroup 333 | - Alpha: 1 334 | Axes Length: 1 335 | Axes Radius: 0.10000000149011612 336 | Class: rviz/PoseWithCovariance 337 | Color: 255; 25; 0 338 | Covariance: 339 | Orientation: 340 | Alpha: 0.5 341 | Color: 255; 255; 127 342 | Color Style: Unique 343 | Frame: Local 344 | Offset: 1 345 | Scale: 1 346 | Value: true 347 | Position: 348 | Alpha: 0.30000001192092896 349 | Color: 204; 51; 204 350 | Scale: 1 351 | Value: true 352 | Value: true 353 | Enabled: false 354 | Head Length: 0.30000001192092896 355 | Head Radius: 0.10000000149011612 356 | Name: PoseWithCovariance 357 | Shaft Length: 1 358 | Shaft Radius: 0.05000000074505806 359 | Shape: Arrow 360 | Topic: /a1/pose_with_cov 361 | Unreliable: false 362 | Value: false 363 | - Alpha: 1 364 | Autocompute Intensity Bounds: true 365 | Class: grid_map_rviz_plugin/GridMap 366 | Color: 200; 200; 200 367 | Color Layer: elevation 368 | Color Transformer: GridMapLayer 369 | Enabled: true 370 | Height Layer: elevation 371 | Height Transformer: GridMapLayer 372 | History Length: 1 373 | Invert Rainbow: false 374 | Max Color: 255; 255; 255 375 | Max Intensity: 10 376 | Min Color: 0; 0; 0 377 | Min Intensity: 0 378 | Name: GridMap 379 | Show Grid Lines: true 380 | Topic: /elevation_mapping/elevation_map_raw 381 | Unreliable: false 382 | Use Rainbow: true 383 | Value: true 384 | - Class: rviz/TF 385 | Enabled: true 386 | Frame Timeout: 15 387 | Frames: 388 | All Enabled: false 389 | Marker Scale: 1 390 | Name: TF 391 | Show Arrows: true 392 | Show Axes: true 393 | Show Names: true 394 | Tree: 395 | {} 396 | Update Interval: 0 397 | Value: true 398 | - Alpha: 1 399 | Autocompute Intensity Bounds: true 400 | Autocompute Value Bounds: 401 | Max Value: 10 402 | Min Value: -10 403 | Value: true 404 | Axis: Z 405 | Channel Name: intensity 406 | Class: rviz/PointCloud2 407 | Color: 255; 255; 255 408 | Color Transformer: RGB8 409 | Decay Time: 0 410 | Enabled: false 411 | Invert Rainbow: false 412 | Max Color: 255; 255; 255 413 | Max Intensity: 4096 414 | Min Color: 0; 0; 0 415 | Min Intensity: 0 416 | Name: PointCloud2 417 | Position Transformer: XYZ 418 | Queue Size: 10 419 | Selectable: true 420 | Size (Pixels): 3 421 | Size (m): 0.009999999776482582 422 | Style: Flat Squares 423 | Topic: /camera_downward/depth/color/points 424 | Unreliable: false 425 | Use Fixed Frame: true 426 | Use rainbow: true 427 | Value: false 428 | - Class: rviz/MarkerArray 429 | Enabled: true 430 | Marker Topic: /a1_robot_visualizer 431 | Name: MarkerArray 432 | Namespaces: 433 | a1_robot: true 434 | Queue Size: 100 435 | Value: true 436 | - Alpha: 1 437 | Axes Length: 1 438 | Axes Radius: 0.10000000149011612 439 | Class: rviz/Pose 440 | Color: 255; 25; 0 441 | Enabled: true 442 | Head Length: 0.30000001192092896 443 | Head Radius: 0.10000000149011612 444 | Name: Pose 445 | Shaft Length: 1 446 | Shaft Radius: 0.05000000074505806 447 | Shape: Arrow 448 | Topic: /mocap_node/mocap/pose 449 | Unreliable: false 450 | Value: true 451 | - Class: rviz/Image 452 | Enabled: false 453 | Image Topic: /vinsfusion/image_track 454 | Max Value: 1 455 | Median window: 5 456 | Min Value: 0 457 | Name: Image 458 | Normalize Range: true 459 | Queue Size: 2 460 | Transport Hint: raw 461 | Unreliable: false 462 | Value: false 463 | - Class: rviz/Group 464 | Displays: 465 | - Class: rviz/Group 466 | Displays: 467 | - Class: rviz/Marker 468 | Enabled: true 469 | Marker Topic: /isaac_a1/foot0/start_pos 470 | Name: Marker 471 | Namespaces: 472 | basic_shapes: true 473 | Queue Size: 100 474 | Value: true 475 | - Class: rviz/Marker 476 | Enabled: true 477 | Marker Topic: /isaac_a1/foot0/swing_path 478 | Name: Marker 479 | Namespaces: 480 | basic_shapes: true 481 | Queue Size: 100 482 | Value: true 483 | - Class: rviz/Marker 484 | Enabled: true 485 | Marker Topic: /isaac_a1/foot0/end_pos 486 | Name: Marker 487 | Namespaces: 488 | basic_shapes: true 489 | Queue Size: 100 490 | Value: true 491 | Enabled: true 492 | Name: foot0 493 | - Class: rviz/Group 494 | Displays: 495 | - Class: rviz/Marker 496 | Enabled: true 497 | Marker Topic: /isaac_a1/foot1/end_pos 498 | Name: Marker 499 | Namespaces: 500 | basic_shapes: true 501 | Queue Size: 100 502 | Value: true 503 | - Class: rviz/Marker 504 | Enabled: true 505 | Marker Topic: /isaac_a1/foot1/start_pos 506 | Name: Marker 507 | Namespaces: 508 | basic_shapes: true 509 | Queue Size: 100 510 | Value: true 511 | - Class: rviz/Marker 512 | Enabled: true 513 | Marker Topic: /isaac_a1/foot1/swing_path 514 | Name: Marker 515 | Namespaces: 516 | basic_shapes: true 517 | Queue Size: 100 518 | Value: true 519 | Enabled: true 520 | Name: foot1 521 | - Class: rviz/Group 522 | Displays: 523 | - Class: rviz/Marker 524 | Enabled: true 525 | Marker Topic: /isaac_a1/foot2/end_pos 526 | Name: Marker 527 | Namespaces: 528 | basic_shapes: true 529 | Queue Size: 100 530 | Value: true 531 | - Class: rviz/Marker 532 | Enabled: true 533 | Marker Topic: /isaac_a1/foot2/start_pos 534 | Name: Marker 535 | Namespaces: 536 | basic_shapes: true 537 | Queue Size: 100 538 | Value: true 539 | - Class: rviz/Marker 540 | Enabled: true 541 | Marker Topic: /isaac_a1/foot2/swing_path 542 | Name: Marker 543 | Namespaces: 544 | basic_shapes: true 545 | Queue Size: 100 546 | Value: true 547 | Enabled: true 548 | Name: foot2 549 | - Class: rviz/Group 550 | Displays: 551 | - Class: rviz/Marker 552 | Enabled: true 553 | Marker Topic: /isaac_a1/foot3/end_pos 554 | Name: Marker 555 | Namespaces: 556 | basic_shapes: true 557 | Queue Size: 100 558 | Value: true 559 | - Class: rviz/Marker 560 | Enabled: true 561 | Marker Topic: /isaac_a1/foot3/start_pos 562 | Name: Marker 563 | Namespaces: 564 | basic_shapes: true 565 | Queue Size: 100 566 | Value: true 567 | - Class: rviz/Marker 568 | Enabled: true 569 | Marker Topic: /isaac_a1/foot3/swing_path 570 | Name: Marker 571 | Namespaces: 572 | basic_shapes: true 573 | Queue Size: 100 574 | Value: true 575 | Enabled: true 576 | Name: foot3 577 | Enabled: true 578 | Name: foot_paths 579 | Enabled: true 580 | Global Options: 581 | Background Color: 0; 0; 0 582 | Default Light: true 583 | Fixed Frame: a1_world 584 | Frame Rate: 30 585 | Name: root 586 | Tools: 587 | - Class: rviz/MoveCamera 588 | - Class: rviz/Select 589 | - Class: rviz/FocusCamera 590 | - Class: rviz/Measure 591 | - Class: rviz/SetInitialPose 592 | Theta std deviation: 0.2617993950843811 593 | Topic: /initialpose 594 | X std deviation: 0.5 595 | Y std deviation: 0.5 596 | - Class: rviz/SetGoal 597 | Topic: /move_base_simple/goal 598 | - Class: rviz/PublishPoint 599 | Single click: true 600 | Topic: /clicked_point 601 | Value: true 602 | Views: 603 | Current: 604 | Class: rviz/ThirdPersonFollower 605 | Distance: 1.1724423170089722 606 | Enable Stereo Rendering: 607 | Stereo Eye Separation: 0.05999999865889549 608 | Stereo Focal Distance: 1 609 | Swap Stereo Eyes: false 610 | Value: false 611 | Focal Point: 612 | X: 0.35923802852630615 613 | Y: 0.8127052783966064 614 | Z: 0.3814394772052765 615 | Focal Shape Fixed Size: true 616 | Focal Shape Size: 0.05000000074505806 617 | Invert Z Axis: false 618 | Name: Current View 619 | Near Clip Distance: 0.009999999776482582 620 | Pitch: 0.28979769349098206 621 | Target Frame: world 622 | Value: ThirdPersonFollower (rviz) 623 | Yaw: 1.457450270652771 624 | Saved: ~ 625 | Window Geometry: 626 | Displays: 627 | collapsed: false 628 | Height: 764 629 | Hide Left Dock: true 630 | Hide Right Dock: true 631 | Image: 632 | collapsed: true 633 | QMainWindow State: 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 634 | Selection: 635 | collapsed: false 636 | Time: 637 | collapsed: false 638 | Tool Properties: 639 | collapsed: false 640 | Views: 641 | collapsed: true 642 | Width: 1267 643 | X: 2041 644 | Y: 1216 645 | loop_match_Image: 646 | collapsed: false 647 | track_image: 648 | collapsed: false 649 | -------------------------------------------------------------------------------- /config/vilo_rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /VIOGroup1/VIOPath1 9 | - /VIOGroup1/CameraMarker1 10 | - /VIOGroup1/PointCloud1 11 | - /VIOGroup1/HistoryPointCloud1 12 | - /VIOGroup1/track_image1 13 | - /GlobalGroup1/CarModel1 14 | - /TF1/Frames1 15 | - /Image1 16 | Splitter Ratio: 0.4651159942150116 17 | Tree Height: 412 18 | - Class: rviz/Selection 19 | Name: Selection 20 | - Class: rviz/Tool Properties 21 | Expanded: 22 | - /2D Pose Estimate1 23 | - /2D Nav Goal1 24 | - /Publish Point1 25 | Name: Tool Properties 26 | Splitter Ratio: 0.5886790156364441 27 | - Class: rviz/Views 28 | Expanded: 29 | - /Current View1 30 | Name: Views 31 | Splitter Ratio: 0.5 32 | - Class: rviz/Time 33 | Experimental: false 34 | Name: Time 35 | SyncMode: 0 36 | SyncSource: "" 37 | - Class: rviz/Displays 38 | Help Height: 78 39 | Name: Displays 40 | Property Tree Widget: 41 | Expanded: ~ 42 | Splitter Ratio: 0.5 43 | Tree Height: 363 44 | Preferences: 45 | PromptSaveOnExit: true 46 | Toolbars: 47 | toolButtonStyle: 2 48 | Visualization Manager: 49 | Class: "" 50 | Displays: 51 | - Alpha: 0.5 52 | Cell Size: 1 53 | Class: rviz/Grid 54 | Color: 160; 160; 164 55 | Enabled: true 56 | Line Style: 57 | Line Width: 0.029999999329447746 58 | Value: Lines 59 | Name: Grid 60 | Normal Cell Count: 0 61 | Offset: 62 | X: 0 63 | Y: 0 64 | Z: 0 65 | Plane: XY 66 | Plane Cell Count: 10 67 | Reference Frame: 68 | Value: true 69 | - Class: rviz/Axes 70 | Enabled: true 71 | Length: 0.5 72 | Name: Axes 73 | Radius: 0.05000000074505806 74 | Reference Frame: 75 | Value: true 76 | - Class: rviz/Group 77 | Displays: 78 | - Class: rviz/MarkerArray 79 | Enabled: true 80 | Marker Topic: /loop_fusion/pose_graph 81 | Name: loopLink 82 | Namespaces: 83 | {} 84 | Queue Size: 100 85 | Value: true 86 | - Class: rviz/MarkerArray 87 | Enabled: true 88 | Marker Topic: /loop_fusion/camera_pose_visual 89 | Name: loopCamera 90 | Namespaces: 91 | {} 92 | Queue Size: 100 93 | Value: true 94 | - Alpha: 1 95 | Buffer Length: 1 96 | Class: rviz/Path 97 | Color: 204; 0; 0 98 | Enabled: true 99 | Head Diameter: 0.30000001192092896 100 | Head Length: 0.20000000298023224 101 | Length: 0.30000001192092896 102 | Line Style: Lines 103 | Line Width: 0.03999999910593033 104 | Name: loopPath 105 | Offset: 106 | X: 0 107 | Y: 0 108 | Z: 0 109 | Pose Color: 255; 85; 255 110 | Pose Style: None 111 | Radius: 0.029999999329447746 112 | Shaft Diameter: 0.10000000149011612 113 | Shaft Length: 0.10000000149011612 114 | Topic: /loop_fusion/pose_graph_path 115 | Unreliable: false 116 | Value: true 117 | - Alpha: 1 118 | Autocompute Intensity Bounds: true 119 | Autocompute Value Bounds: 120 | Max Value: -0.00362956034950912 121 | Min Value: -0.15546569228172302 122 | Value: true 123 | Axis: Z 124 | Channel Name: intensity 125 | Class: rviz/PointCloud 126 | Color: 255; 255; 255 127 | Color Transformer: FlatColor 128 | Decay Time: 10 129 | Enabled: false 130 | Invert Rainbow: false 131 | Max Color: 255; 255; 255 132 | Max Intensity: 4096 133 | Min Color: 0; 0; 0 134 | Min Intensity: 0 135 | Name: PointCloud 136 | Position Transformer: XYZ 137 | Queue Size: 10 138 | Selectable: true 139 | Size (Pixels): 2 140 | Size (m): 0.009999999776482582 141 | Style: Points 142 | Topic: /loop_fusion/margin_cloud_loop_rect 143 | Unreliable: false 144 | Use Fixed Frame: true 145 | Use rainbow: true 146 | Value: false 147 | - Alpha: 1 148 | Autocompute Intensity Bounds: true 149 | Autocompute Value Bounds: 150 | Max Value: 10 151 | Min Value: -10 152 | Value: true 153 | Axis: Z 154 | Channel Name: intensity 155 | Class: rviz/PointCloud 156 | Color: 0; 255; 0 157 | Color Transformer: FlatColor 158 | Decay Time: 0 159 | Enabled: false 160 | Invert Rainbow: false 161 | Max Color: 255; 255; 255 162 | Max Intensity: 4096 163 | Min Color: 0; 0; 0 164 | Min Intensity: 0 165 | Name: PointCloud 166 | Position Transformer: XYZ 167 | Queue Size: 10 168 | Selectable: true 169 | Size (Pixels): 4 170 | Size (m): 0.009999999776482582 171 | Style: Points 172 | Topic: /loop_fusion/point_cloud_loop_rect 173 | Unreliable: false 174 | Use Fixed Frame: true 175 | Use rainbow: true 176 | Value: false 177 | - Class: rviz/Image 178 | Enabled: false 179 | Image Topic: /loop_fusion/match_image 180 | Max Value: 1 181 | Median window: 5 182 | Min Value: 0 183 | Name: loop_match_Image 184 | Normalize Range: true 185 | Queue Size: 2 186 | Transport Hint: raw 187 | Unreliable: false 188 | Value: false 189 | Enabled: true 190 | Name: LoopGroup 191 | - Class: rviz/Group 192 | Displays: 193 | - Alpha: 1 194 | Buffer Length: 1 195 | Class: rviz/Path 196 | Color: 25; 255; 0 197 | Enabled: true 198 | Head Diameter: 0.30000001192092896 199 | Head Length: 0.20000000298023224 200 | Length: 0.30000001192092896 201 | Line Style: Lines 202 | Line Width: 0.029999999329447746 203 | Name: VIOPath 204 | Offset: 205 | X: 0 206 | Y: 0 207 | Z: 0 208 | Pose Color: 255; 85; 255 209 | Pose Style: None 210 | Radius: 0.029999999329447746 211 | Shaft Diameter: 0.10000000149011612 212 | Shaft Length: 0.10000000149011612 213 | Topic: /vileom/path 214 | Unreliable: false 215 | Value: true 216 | - Class: rviz/MarkerArray 217 | Enabled: true 218 | Marker Topic: /vileom/camera_pose_visual 219 | Name: CameraMarker 220 | Namespaces: 221 | {} 222 | Queue Size: 100 223 | Value: true 224 | - Alpha: 1 225 | Autocompute Intensity Bounds: true 226 | Autocompute Value Bounds: 227 | Max Value: 10 228 | Min Value: -10 229 | Value: true 230 | Axis: Z 231 | Channel Name: intensity 232 | Class: rviz/PointCloud 233 | Color: 255; 255; 255 234 | Color Transformer: Intensity 235 | Decay Time: 0 236 | Enabled: true 237 | Invert Rainbow: false 238 | Max Color: 255; 255; 255 239 | Max Intensity: 4096 240 | Min Color: 0; 0; 0 241 | Min Intensity: 0 242 | Name: PointCloud 243 | Position Transformer: XYZ 244 | Queue Size: 10 245 | Selectable: true 246 | Size (Pixels): 2 247 | Size (m): 0.009999999776482582 248 | Style: Points 249 | Topic: /vileom/point_cloud 250 | Unreliable: false 251 | Use Fixed Frame: true 252 | Use rainbow: true 253 | Value: true 254 | - Alpha: 1 255 | Autocompute Intensity Bounds: true 256 | Autocompute Value Bounds: 257 | Max Value: 10 258 | Min Value: -10 259 | Value: true 260 | Axis: Z 261 | Channel Name: intensity 262 | Class: rviz/PointCloud 263 | Color: 0; 255; 0 264 | Color Transformer: FlatColor 265 | Decay Time: 100 266 | Enabled: true 267 | Invert Rainbow: false 268 | Max Color: 255; 255; 255 269 | Max Intensity: 4096 270 | Min Color: 0; 0; 0 271 | Min Intensity: 0 272 | Name: HistoryPointCloud 273 | Position Transformer: XYZ 274 | Queue Size: 10 275 | Selectable: true 276 | Size (Pixels): 1 277 | Size (m): 0.009999999776482582 278 | Style: Points 279 | Topic: /vileom/margin_cloud 280 | Unreliable: false 281 | Use Fixed Frame: true 282 | Use rainbow: true 283 | Value: true 284 | - Class: rviz/Image 285 | Enabled: false 286 | Image Topic: /vinsfusion/image_track 287 | Max Value: 1 288 | Median window: 5 289 | Min Value: 0 290 | Name: track_image 291 | Normalize Range: true 292 | Queue Size: 2 293 | Transport Hint: raw 294 | Unreliable: false 295 | Value: false 296 | Enabled: true 297 | Name: VIOGroup 298 | - Class: rviz/Group 299 | Displays: 300 | - Alpha: 1 301 | Buffer Length: 1 302 | Class: rviz/Path 303 | Color: 0; 0; 255 304 | Enabled: true 305 | Head Diameter: 0.30000001192092896 306 | Head Length: 0.20000000298023224 307 | Length: 0.30000001192092896 308 | Line Style: Lines 309 | Line Width: 0.029999999329447746 310 | Name: GloablPath 311 | Offset: 312 | X: 0 313 | Y: 0 314 | Z: 0 315 | Pose Color: 255; 85; 255 316 | Pose Style: None 317 | Radius: 0.029999999329447746 318 | Shaft Diameter: 0.10000000149011612 319 | Shaft Length: 0.10000000149011612 320 | Topic: /globalEstimator/global_path 321 | Unreliable: false 322 | Value: true 323 | - Class: rviz/MarkerArray 324 | Enabled: true 325 | Marker Topic: /globalEstimator/car_model 326 | Name: CarModel 327 | Namespaces: 328 | {} 329 | Queue Size: 100 330 | Value: true 331 | Enabled: true 332 | Name: GlobalGroup 333 | - Alpha: 1 334 | Axes Length: 1 335 | Axes Radius: 0.10000000149011612 336 | Class: rviz/PoseWithCovariance 337 | Color: 255; 25; 0 338 | Covariance: 339 | Orientation: 340 | Alpha: 0.5 341 | Color: 255; 255; 127 342 | Color Style: Unique 343 | Frame: Local 344 | Offset: 1 345 | Scale: 1 346 | Value: true 347 | Position: 348 | Alpha: 0.30000001192092896 349 | Color: 204; 51; 204 350 | Scale: 1 351 | Value: true 352 | Value: true 353 | Enabled: false 354 | Head Length: 0.30000001192092896 355 | Head Radius: 0.10000000149011612 356 | Name: PoseWithCovariance 357 | Shaft Length: 1 358 | Shaft Radius: 0.05000000074505806 359 | Shape: Arrow 360 | Topic: /a1/pose_with_cov 361 | Unreliable: false 362 | Value: false 363 | - Alpha: 1 364 | Autocompute Intensity Bounds: true 365 | Class: grid_map_rviz_plugin/GridMap 366 | Color: 200; 200; 200 367 | Color Layer: elevation 368 | Color Transformer: GridMapLayer 369 | Enabled: true 370 | Height Layer: elevation 371 | Height Transformer: GridMapLayer 372 | History Length: 1 373 | Invert Rainbow: false 374 | Max Color: 255; 255; 255 375 | Max Intensity: 10 376 | Min Color: 0; 0; 0 377 | Min Intensity: 0 378 | Name: GridMap 379 | Show Grid Lines: true 380 | Topic: /elevation_mapping/elevation_map_raw 381 | Unreliable: false 382 | Use Rainbow: true 383 | Value: true 384 | - Class: rviz/TF 385 | Enabled: true 386 | Frame Timeout: 15 387 | Frames: 388 | All Enabled: false 389 | Marker Scale: 1 390 | Name: TF 391 | Show Arrows: true 392 | Show Axes: true 393 | Show Names: true 394 | Tree: 395 | {} 396 | Update Interval: 0 397 | Value: true 398 | - Alpha: 1 399 | Autocompute Intensity Bounds: true 400 | Autocompute Value Bounds: 401 | Max Value: 10 402 | Min Value: -10 403 | Value: true 404 | Axis: Z 405 | Channel Name: intensity 406 | Class: rviz/PointCloud2 407 | Color: 255; 255; 255 408 | Color Transformer: RGB8 409 | Decay Time: 0 410 | Enabled: false 411 | Invert Rainbow: false 412 | Max Color: 255; 255; 255 413 | Max Intensity: 4096 414 | Min Color: 0; 0; 0 415 | Min Intensity: 0 416 | Name: PointCloud2 417 | Position Transformer: XYZ 418 | Queue Size: 10 419 | Selectable: true 420 | Size (Pixels): 3 421 | Size (m): 0.009999999776482582 422 | Style: Flat Squares 423 | Topic: /camera_downward/depth/color/points 424 | Unreliable: false 425 | Use Fixed Frame: true 426 | Use rainbow: true 427 | Value: false 428 | - Class: rviz/MarkerArray 429 | Enabled: true 430 | Marker Topic: /a1_robot_visualizer 431 | Name: MarkerArray 432 | Namespaces: 433 | {} 434 | Queue Size: 100 435 | Value: true 436 | - Alpha: 1 437 | Axes Length: 1 438 | Axes Radius: 0.10000000149011612 439 | Class: rviz/Pose 440 | Color: 255; 25; 0 441 | Enabled: true 442 | Head Length: 0.30000001192092896 443 | Head Radius: 0.10000000149011612 444 | Name: Pose 445 | Shaft Length: 1 446 | Shaft Radius: 0.05000000074505806 447 | Shape: Arrow 448 | Topic: /mocap_node/mocap/pose 449 | Unreliable: false 450 | Value: true 451 | - Class: rviz/Image 452 | Enabled: true 453 | Image Topic: /vinsfusion/image_track 454 | Max Value: 1 455 | Median window: 5 456 | Min Value: 0 457 | Name: Image 458 | Normalize Range: true 459 | Queue Size: 2 460 | Transport Hint: raw 461 | Unreliable: false 462 | Value: true 463 | Enabled: true 464 | Global Options: 465 | Background Color: 0; 0; 0 466 | Default Light: true 467 | Fixed Frame: a1_world 468 | Frame Rate: 30 469 | Name: root 470 | Tools: 471 | - Class: rviz/MoveCamera 472 | - Class: rviz/Select 473 | - Class: rviz/FocusCamera 474 | - Class: rviz/Measure 475 | - Class: rviz/SetInitialPose 476 | Theta std deviation: 0.2617993950843811 477 | Topic: /initialpose 478 | X std deviation: 0.5 479 | Y std deviation: 0.5 480 | - Class: rviz/SetGoal 481 | Topic: /move_base_simple/goal 482 | - Class: rviz/PublishPoint 483 | Single click: true 484 | Topic: /clicked_point 485 | Value: true 486 | Views: 487 | Current: 488 | Class: rviz/ThirdPersonFollower 489 | Distance: 4.171115398406982 490 | Enable Stereo Rendering: 491 | Stereo Eye Separation: 0.05999999865889549 492 | Stereo Focal Distance: 1 493 | Swap Stereo Eyes: false 494 | Value: false 495 | Focal Point: 496 | X: -1.6568589210510254 497 | Y: -2.260728359222412 498 | Z: 0.38143816590309143 499 | Focal Shape Fixed Size: true 500 | Focal Shape Size: 0.05000000074505806 501 | Invert Z Axis: false 502 | Name: Current View 503 | Near Clip Distance: 0.009999999776482582 504 | Pitch: 1.459796667098999 505 | Target Frame: world 506 | Value: ThirdPersonFollower (rviz) 507 | Yaw: 4.835631847381592 508 | Saved: ~ 509 | Window Geometry: 510 | Displays: 511 | collapsed: false 512 | Height: 986 513 | Hide Left Dock: false 514 | Hide Right Dock: true 515 | Image: 516 | collapsed: false 517 | QMainWindow State: 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 518 | Selection: 519 | collapsed: false 520 | Time: 521 | collapsed: false 522 | Tool Properties: 523 | collapsed: false 524 | Views: 525 | collapsed: true 526 | Width: 1410 527 | X: 1125 528 | Y: 164 529 | loop_match_Image: 530 | collapsed: false 531 | track_image: 532 | collapsed: false 533 | -------------------------------------------------------------------------------- /config/vins_rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /VIOGroup1 10 | - /VIOGroup1/VIOPath1 11 | - /VIOGroup1/CameraMarker1 12 | - /VIOGroup1/PointCloud1 13 | - /VIOGroup1/HistoryPointCloud1 14 | - /VIOGroup1/track_image1 15 | - /GlobalGroup1/CarModel1 16 | - /TF1/Frames1 17 | - /Image1 18 | Splitter Ratio: 0.4651159942150116 19 | Tree Height: 592 20 | - Class: rviz/Selection 21 | Name: Selection 22 | - Class: rviz/Tool Properties 23 | Expanded: 24 | - /2D Pose Estimate1 25 | - /2D Nav Goal1 26 | - /Publish Point1 27 | Name: Tool Properties 28 | Splitter Ratio: 0.5886790156364441 29 | - Class: rviz/Views 30 | Expanded: 31 | - /Current View1 32 | Name: Views 33 | Splitter Ratio: 0.5 34 | - Class: rviz/Time 35 | Experimental: false 36 | Name: Time 37 | SyncMode: 0 38 | SyncSource: Image 39 | - Class: rviz/Displays 40 | Help Height: 78 41 | Name: Displays 42 | Property Tree Widget: 43 | Expanded: ~ 44 | Splitter Ratio: 0.5 45 | Tree Height: 363 46 | Preferences: 47 | PromptSaveOnExit: true 48 | Toolbars: 49 | toolButtonStyle: 2 50 | Visualization Manager: 51 | Class: "" 52 | Displays: 53 | - Alpha: 0.5 54 | Cell Size: 1 55 | Class: rviz/Grid 56 | Color: 160; 160; 164 57 | Enabled: true 58 | Line Style: 59 | Line Width: 0.029999999329447746 60 | Value: Lines 61 | Name: Grid 62 | Normal Cell Count: 0 63 | Offset: 64 | X: 0 65 | Y: 0 66 | Z: 0 67 | Plane: XY 68 | Plane Cell Count: 10 69 | Reference Frame: 70 | Value: true 71 | - Class: rviz/Axes 72 | Enabled: true 73 | Length: 0.5 74 | Name: Axes 75 | Radius: 0.05000000074505806 76 | Reference Frame: 77 | Value: true 78 | - Class: rviz/Group 79 | Displays: 80 | - Class: rviz/MarkerArray 81 | Enabled: true 82 | Marker Topic: /loop_fusion/pose_graph 83 | Name: loopLink 84 | Namespaces: 85 | {} 86 | Queue Size: 100 87 | Value: true 88 | - Class: rviz/MarkerArray 89 | Enabled: true 90 | Marker Topic: /loop_fusion/camera_pose_visual 91 | Name: loopCamera 92 | Namespaces: 93 | {} 94 | Queue Size: 100 95 | Value: true 96 | - Alpha: 1 97 | Buffer Length: 1 98 | Class: rviz/Path 99 | Color: 204; 0; 0 100 | Enabled: true 101 | Head Diameter: 0.30000001192092896 102 | Head Length: 0.20000000298023224 103 | Length: 0.30000001192092896 104 | Line Style: Lines 105 | Line Width: 0.03999999910593033 106 | Name: loopPath 107 | Offset: 108 | X: 0 109 | Y: 0 110 | Z: 0 111 | Pose Color: 255; 85; 255 112 | Pose Style: None 113 | Radius: 0.029999999329447746 114 | Shaft Diameter: 0.10000000149011612 115 | Shaft Length: 0.10000000149011612 116 | Topic: /loop_fusion/pose_graph_path 117 | Unreliable: false 118 | Value: true 119 | - Alpha: 1 120 | Autocompute Intensity Bounds: true 121 | Autocompute Value Bounds: 122 | Max Value: -0.00362956034950912 123 | Min Value: -0.15546569228172302 124 | Value: true 125 | Axis: Z 126 | Channel Name: intensity 127 | Class: rviz/PointCloud 128 | Color: 255; 255; 255 129 | Color Transformer: FlatColor 130 | Decay Time: 10 131 | Enabled: false 132 | Invert Rainbow: false 133 | Max Color: 255; 255; 255 134 | Max Intensity: 4096 135 | Min Color: 0; 0; 0 136 | Min Intensity: 0 137 | Name: PointCloud 138 | Position Transformer: XYZ 139 | Queue Size: 10 140 | Selectable: true 141 | Size (Pixels): 2 142 | Size (m): 0.009999999776482582 143 | Style: Points 144 | Topic: /loop_fusion/margin_cloud_loop_rect 145 | Unreliable: false 146 | Use Fixed Frame: true 147 | Use rainbow: true 148 | Value: false 149 | - Alpha: 1 150 | Autocompute Intensity Bounds: true 151 | Autocompute Value Bounds: 152 | Max Value: 10 153 | Min Value: -10 154 | Value: true 155 | Axis: Z 156 | Channel Name: intensity 157 | Class: rviz/PointCloud 158 | Color: 0; 255; 0 159 | Color Transformer: FlatColor 160 | Decay Time: 0 161 | Enabled: false 162 | Invert Rainbow: false 163 | Max Color: 255; 255; 255 164 | Max Intensity: 4096 165 | Min Color: 0; 0; 0 166 | Min Intensity: 0 167 | Name: PointCloud 168 | Position Transformer: XYZ 169 | Queue Size: 10 170 | Selectable: true 171 | Size (Pixels): 4 172 | Size (m): 0.009999999776482582 173 | Style: Points 174 | Topic: /loop_fusion/point_cloud_loop_rect 175 | Unreliable: false 176 | Use Fixed Frame: true 177 | Use rainbow: true 178 | Value: false 179 | - Class: rviz/Image 180 | Enabled: false 181 | Image Topic: /loop_fusion/match_image 182 | Max Value: 1 183 | Median window: 5 184 | Min Value: 0 185 | Name: loop_match_Image 186 | Normalize Range: true 187 | Queue Size: 2 188 | Transport Hint: raw 189 | Unreliable: false 190 | Value: false 191 | Enabled: true 192 | Name: LoopGroup 193 | - Class: rviz/Group 194 | Displays: 195 | - Alpha: 1 196 | Buffer Length: 1 197 | Class: rviz/Path 198 | Color: 25; 255; 0 199 | Enabled: true 200 | Head Diameter: 0.30000001192092896 201 | Head Length: 0.20000000298023224 202 | Length: 0.30000001192092896 203 | Line Style: Lines 204 | Line Width: 0.029999999329447746 205 | Name: VIOPath 206 | Offset: 207 | X: 0 208 | Y: 0 209 | Z: 0 210 | Pose Color: 255; 85; 255 211 | Pose Style: None 212 | Radius: 0.029999999329447746 213 | Shaft Diameter: 0.10000000149011612 214 | Shaft Length: 0.10000000149011612 215 | Topic: /vinsfusion/path 216 | Unreliable: false 217 | Value: true 218 | - Class: rviz/MarkerArray 219 | Enabled: true 220 | Marker Topic: /vinsfusion/camera_pose_visual 221 | Name: CameraMarker 222 | Namespaces: 223 | CameraPoseVisualization: true 224 | Queue Size: 100 225 | Value: true 226 | - Alpha: 1 227 | Autocompute Intensity Bounds: true 228 | Autocompute Value Bounds: 229 | Max Value: 10 230 | Min Value: -10 231 | Value: true 232 | Axis: Z 233 | Channel Name: intensity 234 | Class: rviz/PointCloud 235 | Color: 255; 255; 255 236 | Color Transformer: Intensity 237 | Decay Time: 0 238 | Enabled: true 239 | Invert Rainbow: false 240 | Max Color: 255; 255; 255 241 | Max Intensity: 4096 242 | Min Color: 0; 0; 0 243 | Min Intensity: 0 244 | Name: PointCloud 245 | Position Transformer: XYZ 246 | Queue Size: 10 247 | Selectable: true 248 | Size (Pixels): 2 249 | Size (m): 0.009999999776482582 250 | Style: Points 251 | Topic: /vileom/point_cloud 252 | Unreliable: false 253 | Use Fixed Frame: true 254 | Use rainbow: true 255 | Value: true 256 | - Alpha: 1 257 | Autocompute Intensity Bounds: true 258 | Autocompute Value Bounds: 259 | Max Value: 10 260 | Min Value: -10 261 | Value: true 262 | Axis: Z 263 | Channel Name: intensity 264 | Class: rviz/PointCloud 265 | Color: 0; 255; 0 266 | Color Transformer: FlatColor 267 | Decay Time: 100 268 | Enabled: true 269 | Invert Rainbow: false 270 | Max Color: 255; 255; 255 271 | Max Intensity: 4096 272 | Min Color: 0; 0; 0 273 | Min Intensity: 0 274 | Name: HistoryPointCloud 275 | Position Transformer: XYZ 276 | Queue Size: 10 277 | Selectable: true 278 | Size (Pixels): 1 279 | Size (m): 0.009999999776482582 280 | Style: Points 281 | Topic: /vileom/margin_cloud 282 | Unreliable: false 283 | Use Fixed Frame: true 284 | Use rainbow: true 285 | Value: true 286 | - Class: rviz/Image 287 | Enabled: false 288 | Image Topic: /vinsfusion/image_track 289 | Max Value: 1 290 | Median window: 5 291 | Min Value: 0 292 | Name: track_image 293 | Normalize Range: true 294 | Queue Size: 2 295 | Transport Hint: raw 296 | Unreliable: false 297 | Value: false 298 | Enabled: true 299 | Name: VIOGroup 300 | - Class: rviz/Group 301 | Displays: 302 | - Alpha: 1 303 | Buffer Length: 1 304 | Class: rviz/Path 305 | Color: 0; 0; 255 306 | Enabled: true 307 | Head Diameter: 0.30000001192092896 308 | Head Length: 0.20000000298023224 309 | Length: 0.30000001192092896 310 | Line Style: Lines 311 | Line Width: 0.029999999329447746 312 | Name: GloablPath 313 | Offset: 314 | X: 0 315 | Y: 0 316 | Z: 0 317 | Pose Color: 255; 85; 255 318 | Pose Style: None 319 | Radius: 0.029999999329447746 320 | Shaft Diameter: 0.10000000149011612 321 | Shaft Length: 0.10000000149011612 322 | Topic: /globalEstimator/global_path 323 | Unreliable: false 324 | Value: true 325 | - Class: rviz/MarkerArray 326 | Enabled: true 327 | Marker Topic: /globalEstimator/car_model 328 | Name: CarModel 329 | Namespaces: 330 | {} 331 | Queue Size: 100 332 | Value: true 333 | Enabled: true 334 | Name: GlobalGroup 335 | - Alpha: 1 336 | Axes Length: 1 337 | Axes Radius: 0.10000000149011612 338 | Class: rviz/PoseWithCovariance 339 | Color: 255; 25; 0 340 | Covariance: 341 | Orientation: 342 | Alpha: 0.5 343 | Color: 255; 255; 127 344 | Color Style: Unique 345 | Frame: Local 346 | Offset: 1 347 | Scale: 1 348 | Value: true 349 | Position: 350 | Alpha: 0.30000001192092896 351 | Color: 204; 51; 204 352 | Scale: 1 353 | Value: true 354 | Value: true 355 | Enabled: false 356 | Head Length: 0.30000001192092896 357 | Head Radius: 0.10000000149011612 358 | Name: PoseWithCovariance 359 | Shaft Length: 1 360 | Shaft Radius: 0.05000000074505806 361 | Shape: Arrow 362 | Topic: /a1/pose_with_cov 363 | Unreliable: false 364 | Value: false 365 | - Alpha: 1 366 | Autocompute Intensity Bounds: true 367 | Class: grid_map_rviz_plugin/GridMap 368 | Color: 200; 200; 200 369 | Color Layer: elevation 370 | Color Transformer: GridMapLayer 371 | Enabled: true 372 | Height Layer: elevation 373 | Height Transformer: GridMapLayer 374 | History Length: 1 375 | Invert Rainbow: false 376 | Max Color: 255; 255; 255 377 | Max Intensity: 10 378 | Min Color: 0; 0; 0 379 | Min Intensity: 0 380 | Name: GridMap 381 | Show Grid Lines: true 382 | Topic: /elevation_mapping/elevation_map_raw 383 | Unreliable: false 384 | Use Rainbow: true 385 | Value: true 386 | - Class: rviz/TF 387 | Enabled: true 388 | Frame Timeout: 15 389 | Frames: 390 | All Enabled: false 391 | body: 392 | Value: true 393 | camera: 394 | Value: true 395 | world: 396 | Value: true 397 | Marker Scale: 1 398 | Name: TF 399 | Show Arrows: true 400 | Show Axes: true 401 | Show Names: true 402 | Tree: 403 | world: 404 | body: 405 | camera: 406 | {} 407 | Update Interval: 0 408 | Value: true 409 | - Alpha: 1 410 | Autocompute Intensity Bounds: true 411 | Autocompute Value Bounds: 412 | Max Value: 10 413 | Min Value: -10 414 | Value: true 415 | Axis: Z 416 | Channel Name: intensity 417 | Class: rviz/PointCloud2 418 | Color: 255; 255; 255 419 | Color Transformer: RGB8 420 | Decay Time: 0 421 | Enabled: false 422 | Invert Rainbow: false 423 | Max Color: 255; 255; 255 424 | Max Intensity: 4096 425 | Min Color: 0; 0; 0 426 | Min Intensity: 0 427 | Name: PointCloud2 428 | Position Transformer: XYZ 429 | Queue Size: 10 430 | Selectable: true 431 | Size (Pixels): 3 432 | Size (m): 0.009999999776482582 433 | Style: Flat Squares 434 | Topic: /camera_downward/depth/color/points 435 | Unreliable: false 436 | Use Fixed Frame: true 437 | Use rainbow: true 438 | Value: false 439 | - Class: rviz/MarkerArray 440 | Enabled: true 441 | Marker Topic: /a1_robot_visualizer 442 | Name: MarkerArray 443 | Namespaces: 444 | {} 445 | Queue Size: 100 446 | Value: true 447 | - Alpha: 1 448 | Axes Length: 1 449 | Axes Radius: 0.10000000149011612 450 | Class: rviz/Pose 451 | Color: 255; 25; 0 452 | Enabled: true 453 | Head Length: 0.30000001192092896 454 | Head Radius: 0.10000000149011612 455 | Name: Pose 456 | Shaft Length: 1 457 | Shaft Radius: 0.05000000074505806 458 | Shape: Arrow 459 | Topic: /mocap_node/mocap/pose 460 | Unreliable: false 461 | Value: true 462 | - Class: rviz/Image 463 | Enabled: true 464 | Image Topic: /camera_forward/infra1/image_rect_raw 465 | Max Value: 1 466 | Median window: 5 467 | Min Value: 0 468 | Name: Image 469 | Normalize Range: true 470 | Queue Size: 2 471 | Transport Hint: raw 472 | Unreliable: false 473 | Value: true 474 | Enabled: true 475 | Global Options: 476 | Background Color: 0; 0; 0 477 | Default Light: true 478 | Fixed Frame: world 479 | Frame Rate: 30 480 | Name: root 481 | Tools: 482 | - Class: rviz/MoveCamera 483 | - Class: rviz/Select 484 | - Class: rviz/FocusCamera 485 | - Class: rviz/Measure 486 | - Class: rviz/SetInitialPose 487 | Theta std deviation: 0.2617993950843811 488 | Topic: /initialpose 489 | X std deviation: 0.5 490 | Y std deviation: 0.5 491 | - Class: rviz/SetGoal 492 | Topic: /move_base_simple/goal 493 | - Class: rviz/PublishPoint 494 | Single click: true 495 | Topic: /clicked_point 496 | Value: true 497 | Views: 498 | Current: 499 | Class: rviz/ThirdPersonFollower 500 | Distance: 13.094644546508789 501 | Enable Stereo Rendering: 502 | Stereo Eye Separation: 0.05999999865889549 503 | Stereo Focal Distance: 1 504 | Swap Stereo Eyes: false 505 | Value: false 506 | Focal Point: 507 | X: 0.01493990421295166 508 | Y: -1.019273281097412 509 | Z: 0.3814398944377899 510 | Focal Shape Fixed Size: true 511 | Focal Shape Size: 0.05000000074505806 512 | Invert Z Axis: false 513 | Name: Current View 514 | Near Clip Distance: 0.009999999776482582 515 | Pitch: -0.22020240128040314 516 | Target Frame: world 517 | Value: ThirdPersonFollower (rviz) 518 | Yaw: 4.495631217956543 519 | Saved: ~ 520 | Window Geometry: 521 | Displays: 522 | collapsed: false 523 | Height: 1042 524 | Hide Left Dock: false 525 | Hide Right Dock: true 526 | Image: 527 | collapsed: false 528 | QMainWindow State: 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 529 | Selection: 530 | collapsed: false 531 | Time: 532 | collapsed: false 533 | Tool Properties: 534 | collapsed: false 535 | Views: 536 | collapsed: true 537 | Width: 1587 538 | X: 833 539 | Y: 122 540 | loop_match_Image: 541 | collapsed: false 542 | track_image: 543 | collapsed: false 544 | -------------------------------------------------------------------------------- /doc/images/a1_visualizer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShuoYangRobotics/A1_visualizer/d64c958d65b4d0fd1ae09a62affb8b4a8fc4a8f6/doc/images/a1_visualizer.png -------------------------------------------------------------------------------- /launch/isaac_a1_dev.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/isaac_a1_vilo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/isaac_a1_vins.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/vilo_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /launch/vins_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | a1_visualizer 4 | 0.0.0 5 | The visualizer of A1 robot 6 | 7 | 8 | 9 | 10 | Shuo 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | roslib 55 | rosbag 56 | std_msgs 57 | geometry_msgs 58 | transform_graph 59 | message_generation 60 | roscpp 61 | rospy 62 | std_msgs 63 | geometry_msgs 64 | roscpp 65 | rospy 66 | roslib 67 | rosbag 68 | std_msgs 69 | geometry_msgs 70 | transform_graph 71 | message_runtime 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /src/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "", 6 | "limitSymbolsToIncludedHeaders": true 7 | }, 8 | "includePath": [ 9 | "/home/shuoy/nv_work/unitree_ws/devel/include/**", 10 | "/home/shuoy/nv_work/ros_catkin_ws/devel/include/**", 11 | "/home/shuoy/nv_work/ros_catkin_ws/install/include/**", 12 | "/home/shuoy/nv_work/util_ws/devel/include/**", 13 | "/home/shuoy/nv_work/unitree_ws/src/VINS-Fusion/camera_models/include/**", 14 | "/home/shuoy/nv_work/unitree_ws/src/elevation_mapping/elevation_mapping/include/**", 15 | "/home/shuoy/nv_work/unitree_ws/src/elevation_mapping/elevation_mapping_demos/include/**", 16 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_core/include/**", 17 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_costmap_2d/include/**", 18 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_cv/include/**", 19 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_demos/include/**", 20 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_filters/include/**", 21 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_loader/include/**", 22 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_octomap/include/**", 23 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_pcl/include/**", 24 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_ros/include/**", 25 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_rviz_plugin/include/**", 26 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_sdf/include/**", 27 | "/home/shuoy/nv_work/unitree_ws/src/grid_map/grid_map_visualization/include/**", 28 | "/home/shuoy/nv_work/unitree_ws/src/kindr/include/**", 29 | "/home/shuoy/nv_work/unitree_ws/src/kindr_ros/kindr_ros/include/**", 30 | "/home/shuoy/nv_work/unitree_ws/src/kindr_ros/kindr_rviz_plugins/include/**", 31 | "/home/shuoy/nv_work/unitree_ws/src/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/**", 32 | "/home/shuoy/nv_work/util_ws/src/PlotJuggler/include/**", 33 | "/home/shuoy/nv_work/unitree_ws/src/unitree_ros/unitree_controller/include/**", 34 | "/home/shuoy/nv_work/unitree_ws/src/unitree_ros/unitree_legged_control/include/**", 35 | "/home/shuoy/nv_work/unitree_ws/src/unitree_ros/unitree_legged_real/include/**", 36 | "/usr/include/**" 37 | ], 38 | "name": "ROS" 39 | } 40 | ], 41 | "version": 4 42 | } -------------------------------------------------------------------------------- /src/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/shuoy/nv_work/util_ws/devel/lib/python3/dist-packages", 4 | "/home/shuoy/nv_work/unitree_ws/devel/lib/python3/dist-packages", 5 | "/home/shuoy/nv_work/ros_catkin_ws/install/lib/python3/dist-packages", 6 | "/home/shuoy/nv_work/ros_catkin_ws/devel/lib/python3/dist-packages", 7 | "/home/shuoy/nv_work/carbgym/python", 8 | "/home/shuoy/nv_work/laikago_rl" 9 | ], 10 | "python.analysis.extraPaths": [ 11 | "/home/shuoy/nv_work/util_ws/devel/lib/python3/dist-packages", 12 | "/home/shuoy/nv_work/unitree_ws/devel/lib/python3/dist-packages", 13 | "/home/shuoy/nv_work/ros_catkin_ws/install/lib/python3/dist-packages", 14 | "/home/shuoy/nv_work/ros_catkin_ws/devel/lib/python3/dist-packages", 15 | "/home/shuoy/nv_work/carbgym/python", 16 | "/home/shuoy/nv_work/laikago_rl" 17 | ] 18 | } -------------------------------------------------------------------------------- /src/hardware_a1_visualize.cpp: -------------------------------------------------------------------------------- 1 | 2 | // stl 3 | #include 4 | 5 | // ROS 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | // visualizer 32 | #include "robot_markers/builder.h" 33 | 34 | // unitree message 35 | // #include 36 | 37 | 38 | // for visualize robot in rviz robomarker 39 | robot_markers::Builder* builder; 40 | visualization_msgs::MarkerArray a1_viz_array; 41 | ros::Publisher marker_arr_pub; 42 | 43 | // move joint position as global variable 44 | std::map joint_positions; 45 | tf::TransformListener* listener; 46 | 47 | 48 | 49 | void a1_joint_state_callback(const sensor_msgs::JointStateConstPtr& a1_state) 50 | { 51 | // ROS_INFO("received"); 52 | // FL 53 | joint_positions["FL_hip_joint"] = a1_state -> position[0]; 54 | joint_positions["FL_thigh_joint"] = a1_state -> position[1]; 55 | joint_positions["FL_calf_joint"] = a1_state -> position[2]; 56 | // FR 57 | joint_positions["FR_hip_joint"] = a1_state -> position[3]; 58 | joint_positions["FR_thigh_joint"] = a1_state -> position[4]; 59 | joint_positions["FR_calf_joint"] = a1_state -> position[5]; 60 | // RL 61 | joint_positions["RL_hip_joint"] = a1_state -> position[6]; 62 | joint_positions["RL_thigh_joint"] = a1_state -> position[7]; 63 | joint_positions["RL_calf_joint"] = a1_state -> position[8]; 64 | // RR 65 | joint_positions["RR_hip_joint"] = a1_state -> position[9]; 66 | joint_positions["RR_thigh_joint"] = a1_state -> position[10]; 67 | joint_positions["RR_calf_joint"] = a1_state -> position[11]; 68 | 69 | // ROS_INFO("build"); 70 | builder->SetNamespace("a1_robot"); 71 | builder->SetFrameId("a1_world"); 72 | // ROS_INFO("build a1_viz_array"); 73 | builder->Build(&a1_viz_array,1); 74 | 75 | // ROS_INFO("vis_body_pose"); 76 | // listen to tf 77 | tf::StampedTransform transform; 78 | geometry_msgs::Pose vis_body_pose; 79 | try{ 80 | listener -> lookupTransform("/a1_world", "/a1_body", ros::Time(0), transform); 81 | vis_body_pose.orientation.x = transform.getRotation().getX(); 82 | vis_body_pose.orientation.y = transform.getRotation().getY(); 83 | vis_body_pose.orientation.z = transform.getRotation().getZ(); 84 | vis_body_pose.orientation.w = transform.getRotation().getW(); // notice this order 0:w, 1:x, 2:y, 3:z 85 | vis_body_pose.position.x = transform.getOrigin().x(); 86 | vis_body_pose.position.y = transform.getOrigin().y(); 87 | vis_body_pose.position.z = transform.getOrigin().z(); 88 | } 89 | catch (tf::TransformException &ex) { 90 | // ROS_ERROR("%s",ex.what()); 91 | } 92 | 93 | // ROS_INFO("set"); 94 | builder->SetPose(vis_body_pose); 95 | builder->SetJointPositions(joint_positions); 96 | marker_arr_pub.publish(a1_viz_array); 97 | // clear array for next time use 98 | a1_viz_array.markers.clear(); 99 | } 100 | 101 | 102 | int main(int argc, char** argv) { 103 | 104 | ros::init(argc, argv, "a1_visualizer"); 105 | ros::NodeHandle nh; 106 | ros::Rate loop_rate(100); 107 | listener = new tf::TransformListener(); 108 | 109 | // the robot can also publish a message with type sensor_msgs::JointState 110 | ros::Subscriber sub_a1_joint_foot_msg = nh.subscribe("/hardware_a1/joint_foot", 1000, a1_joint_state_callback); 111 | 112 | 113 | // robomarker visualizer 114 | marker_arr_pub = nh.advertise("a1_robot_visualizer", 100); 115 | urdf::Model model; 116 | model.initFile(ros::package::getPath("unitree_legged_real") + "/../robots/a1_description/urdf/a1.urdf"); 117 | builder = new robot_markers::Builder(model); 118 | builder->Init(); 119 | 120 | 121 | // ros spinner 122 | while (ros::ok()) 123 | { 124 | ros::spinOnce(); 125 | loop_rate.sleep(); 126 | } 127 | 128 | 129 | return 0; 130 | } 131 | -------------------------------------------------------------------------------- /src/isaac_a1_visualize.cpp: -------------------------------------------------------------------------------- 1 | 2 | // stl 3 | #include 4 | 5 | // ROS 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | // visualizer 32 | #include "robot_markers/builder.h" 33 | 34 | // unitree message 35 | // #include 36 | 37 | 38 | // for visualize robot in rviz robomarker 39 | robot_markers::Builder* builder; 40 | visualization_msgs::MarkerArray a1_viz_array; 41 | ros::Publisher marker_arr_pub; 42 | 43 | // move joint position as global variable 44 | std::map joint_positions; 45 | tf::TransformListener* listener; 46 | // body pose is global variable 47 | geometry_msgs::Pose vis_body_pose; 48 | 49 | 50 | void a1_joint_state_callback(const sensor_msgs::JointStateConstPtr& a1_state) 51 | { 52 | // ROS_INFO("received"); 53 | // FL 54 | joint_positions["FL_hip_joint"] = a1_state -> position[0]; 55 | joint_positions["FL_thigh_joint"] = a1_state -> position[1]; 56 | joint_positions["FL_calf_joint"] = a1_state -> position[2]; 57 | // FR 58 | joint_positions["FR_hip_joint"] = a1_state -> position[3]; 59 | joint_positions["FR_thigh_joint"] = a1_state -> position[4]; 60 | joint_positions["FR_calf_joint"] = a1_state -> position[5]; 61 | // RL 62 | joint_positions["RL_hip_joint"] = a1_state -> position[6]; 63 | joint_positions["RL_thigh_joint"] = a1_state -> position[7]; 64 | joint_positions["RL_calf_joint"] = a1_state -> position[8]; 65 | // RR 66 | joint_positions["RR_hip_joint"] = a1_state -> position[9]; 67 | joint_positions["RR_thigh_joint"] = a1_state -> position[10]; 68 | joint_positions["RR_calf_joint"] = a1_state -> position[11]; 69 | 70 | // ROS_INFO("build"); 71 | builder->SetNamespace("a1_robot"); 72 | builder->SetFrameId("world"); 73 | // ROS_INFO("build a1_viz_array"); 74 | builder->Build(&a1_viz_array,1); 75 | 76 | // ROS_INFO("set"); 77 | builder->SetPose(vis_body_pose); 78 | builder->SetJointPositions(joint_positions); 79 | marker_arr_pub.publish(a1_viz_array); 80 | // clear array for next time use 81 | a1_viz_array.markers.clear(); 82 | } 83 | 84 | // if a ground truth pose exist 85 | void gt_pose_callback(const nav_msgs::Odometry::ConstPtr &odom) { 86 | // update 87 | vis_body_pose.orientation.x = odom->pose.pose.orientation.x; 88 | vis_body_pose.orientation.y = odom->pose.pose.orientation.y; 89 | vis_body_pose.orientation.z = odom->pose.pose.orientation.z; 90 | vis_body_pose.orientation.w = odom->pose.pose.orientation.w; // notice this order 0:w, 1:x, 2:y, 3:z 91 | vis_body_pose.position.x = odom->pose.pose.position.x; 92 | vis_body_pose.position.y = odom->pose.pose.position.y; 93 | vis_body_pose.position.z = odom->pose.pose.position.z; 94 | 95 | } 96 | 97 | 98 | int main(int argc, char** argv) { 99 | 100 | ros::init(argc, argv, "isaac_a1_visualizer"); 101 | ros::NodeHandle nh; 102 | ros::Rate loop_rate(100); 103 | listener = new tf::TransformListener(); 104 | 105 | // the robot can also publish a message with type sensor_msgs::JointState 106 | ros::Subscriber sub_a1_joint_foot_msg = nh.subscribe("/isaac_a1/joint_foot", 1000, a1_joint_state_callback); 107 | 108 | // if a ground truth pose exist 109 | ros::Subscriber sub_gt_pose_msg = nh.subscribe("/isaac_a1/gt_body_pose", 100, gt_pose_callback); 110 | 111 | // robomarker visualizer 112 | marker_arr_pub = nh.advertise("a1_robot_visualizer", 100); 113 | urdf::Model model; 114 | model.initFile(ros::package::getPath("unitree_legged_real") + "/../robots/a1_description/urdf/a1.urdf"); 115 | builder = new robot_markers::Builder(model); 116 | builder->Init(); 117 | 118 | 119 | // ros spinner 120 | while (ros::ok()) 121 | { 122 | ros::spinOnce(); 123 | loop_rate.sleep(); 124 | } 125 | 126 | 127 | return 0; 128 | } -------------------------------------------------------------------------------- /src/robot_markers/builder.cpp: -------------------------------------------------------------------------------- 1 | #include "builder.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "boost/shared_ptr.hpp" 8 | #include "geometry_msgs/Pose.h" 9 | #include "geometry_msgs/TransformStamped.h" 10 | #include "ros/ros.h" 11 | #include "transform_graph/transform_graph.h" 12 | #include "urdf/model.h" 13 | #include "visualization_msgs/Marker.h" 14 | #include "visualization_msgs/MarkerArray.h" 15 | 16 | #include "Eigen/Eigen" 17 | 18 | using transform_graph::RefFrame; 19 | using transform_graph::Source; 20 | using transform_graph::Target; 21 | using transform_graph::Transform; 22 | using visualization_msgs::Marker; 23 | using visualization_msgs::MarkerArray; 24 | 25 | typedef std::map LinkMap; 26 | 27 | namespace { 28 | geometry_msgs::Pose ToGeometryPose(const urdf::Pose& urdf_pose) { 29 | geometry_msgs::Pose pose; 30 | pose.position.x = urdf_pose.position.x; 31 | pose.position.y = urdf_pose.position.y; 32 | pose.position.z = urdf_pose.position.z; 33 | pose.orientation.w = urdf_pose.rotation.w; 34 | pose.orientation.x = urdf_pose.rotation.x; 35 | pose.orientation.y = urdf_pose.rotation.y; 36 | pose.orientation.z = urdf_pose.rotation.z; 37 | return pose; 38 | } 39 | 40 | geometry_msgs::Pose ToGeometryPose( 41 | const transform_graph::Transform& transform) { 42 | Eigen::Matrix3d rot(transform.matrix().topLeftCorner(3, 3)); 43 | Eigen::Quaterniond q(rot); 44 | geometry_msgs::Pose pose; 45 | pose.position.x = transform.matrix()(0, 3); 46 | pose.position.y = transform.matrix()(1, 3); 47 | pose.position.z = transform.matrix()(2, 3); 48 | pose.orientation.w = q.w(); 49 | pose.orientation.x = q.x(); 50 | pose.orientation.y = q.y(); 51 | pose.orientation.z = q.z(); 52 | return pose; 53 | } 54 | } // namespace 55 | 56 | namespace robot_markers { 57 | Builder::Builder(const urdf::Model& model) 58 | : model_(model), 59 | fk_(model), 60 | tf_graph_(), 61 | frame_id_(""), 62 | stamp_(0), 63 | ns_(""), 64 | pose_(), 65 | color_(), 66 | lifetime_(0), 67 | frame_locked_(false), 68 | has_initialized_(false) { 69 | pose_.orientation.w = 1; 70 | } 71 | 72 | void Builder::Init() { 73 | fk_.Init(); 74 | 75 | // Build initial transform graph 76 | std::vector joints; 77 | std::vector links; 78 | model_.getLinks(links); 79 | for (size_t i = 0; i < links.size(); ++i) { 80 | const urdf::LinkSharedPtr& link_p = links[i]; 81 | const std::string& name = link_p->name; 82 | 83 | if (link_p->parent_joint) { 84 | geometry_msgs::Pose pose = ToGeometryPose( 85 | link_p->parent_joint->parent_to_joint_origin_transform); 86 | const std::string& parent_name = link_p->getParent()->name; 87 | 88 | ROS_INFO("init transform from %s to %s!", parent_name.c_str(), name.c_str()); 89 | tf_graph_.Add(NodeName(name), RefFrame(NodeName(parent_name)), 90 | Transform(pose)); 91 | } 92 | } 93 | has_initialized_ = true; 94 | } 95 | 96 | void Builder::SetJointPositions( 97 | const std::map joint_positions) { 98 | if (!has_initialized_) { 99 | ROS_ERROR( 100 | "You must call Init() before calling any other methods of " 101 | "robot_markers::Builder."); 102 | return; 103 | } 104 | 105 | std::vector transforms; 106 | fk_.GetTransforms(joint_positions, &transforms); 107 | for (size_t i = 0; i < transforms.size(); ++i) { 108 | const geometry_msgs::TransformStamped& tf = transforms[i]; 109 | transform_graph::Transform transform(tf.transform.translation, 110 | tf.transform.rotation); 111 | tf_graph_.Add(NodeName(tf.child_frame_id), 112 | RefFrame(NodeName(tf.header.frame_id)), transform); 113 | } 114 | } 115 | 116 | void Builder::SetFrameId(const std::string& frame_id) { 117 | if (!has_initialized_) { 118 | ROS_ERROR( 119 | "You must call Init() before calling any other methods of " 120 | "robot_markers::Builder."); 121 | return; 122 | } 123 | 124 | frame_id_ = frame_id; 125 | } 126 | void Builder::SetTime(const ros::Time& stamp) { 127 | if (!has_initialized_) { 128 | ROS_ERROR( 129 | "You must call Init() before calling any other methods of " 130 | "robot_markers::Builder."); 131 | return; 132 | } 133 | 134 | stamp_ = stamp; 135 | } 136 | void Builder::SetNamespace(const std::string& ns) { 137 | if (!has_initialized_) { 138 | ROS_ERROR( 139 | "You must call Init() before calling any other methods of " 140 | "robot_markers::Builder."); 141 | return; 142 | } 143 | ns_ = ns; 144 | } 145 | void Builder::SetPose(const geometry_msgs::Pose& pose) { 146 | if (!has_initialized_) { 147 | ROS_ERROR( 148 | "You must call Init() before calling any other methods of " 149 | "robot_markers::Builder."); 150 | return; 151 | } 152 | pose_ = pose; 153 | } 154 | void Builder::SetColor(float r, float g, float b, float a) { 155 | if (!has_initialized_) { 156 | ROS_ERROR( 157 | "You must call Init() before calling any other methods of " 158 | "robot_markers::Builder."); 159 | return; 160 | } 161 | 162 | color_.r = r; 163 | color_.g = g; 164 | color_.b = b; 165 | color_.a = a; 166 | } 167 | 168 | void Builder::Build(MarkerArray* marker_array, int no_color = 0) { 169 | if (!has_initialized_) { 170 | ROS_ERROR( 171 | "You must call Init() before calling any other methods of " 172 | "robot_markers::Builder."); 173 | return; 174 | } 175 | 176 | const std::string& root_name = model_.getRoot()->name; 177 | // std::cout << root_name << std::endl; 178 | 179 | tf_graph_.Add(NodeName(root_name), RefFrame(frame_id_), pose_); 180 | 181 | std::vector links; 182 | model_.getLinks(links); 183 | 184 | // std::cout << links.size() << std::endl; 185 | 186 | for (size_t i = 0; i < links.size(); ++i) { 187 | const urdf::LinkSharedPtr& link_p = links[i]; 188 | const std::string& name = link_p->name; 189 | 190 | // std::cout << name << std::endl; 191 | if (!link_p->visual) { 192 | continue; 193 | } 194 | 195 | // std::cout << "reach " << __FILE__ << __LINE__ << std::endl; 196 | Marker marker; 197 | BuildMarker(*link_p, i, &marker, no_color); 198 | // std::cout << "reach " << __FILE__ << __LINE__ << std::endl; 199 | transform_graph::Transform transform_out; 200 | // std::cout << "reach " << __FILE__ << __LINE__ << std::endl; 201 | geometry_msgs::Pose visual_transform = 202 | ToGeometryPose(link_p->visual->origin); 203 | 204 | // std::cout << visual_transform.position.x << std::endl; 205 | // std::cout << "reach " << __FILE__ << __LINE__ << std::endl; 206 | // ROS_INFO("Find transform from %s to %s!", frame_id_.c_str(), name.c_str()); 207 | bool success = 208 | tf_graph_.DescribePose(visual_transform, Source(NodeName(name)), 209 | Target(frame_id_), &transform_out); 210 | // std::cout << "reach " << __FILE__ << __LINE__ << std::endl; 211 | if (!success) { 212 | ROS_ERROR("No transform from %s to %s!", frame_id_.c_str(), name.c_str()); 213 | continue; 214 | } else { 215 | } 216 | // std::cout << "reach " << __FILE__ << __LINE__ << std::endl; 217 | marker.pose = ToGeometryPose(transform_out); 218 | marker_array->markers.push_back(marker); 219 | // std::cout << "reach " << __FILE__ << __LINE__ << std::endl; 220 | } 221 | } 222 | 223 | void Builder::BuildMarker(const urdf::Link& link, int id, Marker* output, int no_color = 0) { 224 | output->header.frame_id = frame_id_; 225 | output->header.stamp = stamp_; 226 | output->ns = ns_; 227 | output->id = id; 228 | // modify justin huang's original code to read color from URDF 229 | if (no_color == 0) { 230 | output->color.r = link.visual->material->color.r; 231 | output->color.g = link.visual->material->color.g; 232 | output->color.b = link.visual->material->color.b; 233 | output->color.a = link.visual->material->color.a; 234 | } 235 | else { 236 | output->color.r = 0.33+0.5*(no_color-1); 237 | output->color.g = 0.17+0.5*(no_color-1); 238 | output->color.b = 0.45+0.5*(no_color-1); 239 | output->color.a = 1.00; 240 | } 241 | output->lifetime = lifetime_; 242 | output->frame_locked = frame_locked_; 243 | 244 | if (link.visual->geometry->type == urdf::Geometry::BOX) { 245 | output->type = Marker::CUBE; 246 | const urdf::Box& box = dynamic_cast(*link.visual->geometry); 247 | output->scale.x = box.dim.x; 248 | output->scale.y = box.dim.y; 249 | output->scale.z = box.dim.z; 250 | } else if (link.visual->geometry->type == urdf::Geometry::CYLINDER) { 251 | output->type = Marker::CYLINDER; 252 | const urdf::Cylinder& cylinder = 253 | dynamic_cast(*link.visual->geometry); 254 | output->scale.x = 2 * cylinder.radius; 255 | output->scale.y = 2 * cylinder.radius; 256 | output->scale.z = cylinder.length; 257 | } else if (link.visual->geometry->type == urdf::Geometry::MESH) { 258 | output->type = Marker::MESH_RESOURCE; 259 | const urdf::Mesh& mesh = dynamic_cast(*link.visual->geometry); 260 | output->mesh_use_embedded_materials = true; 261 | if (mesh.scale.x == 0 && mesh.scale.y == 0 && mesh.scale.z == 0) { 262 | output->scale.x = 1; 263 | output->scale.y = 1; 264 | output->scale.z = 1; 265 | } else { 266 | output->scale.x = mesh.scale.x; 267 | output->scale.y = mesh.scale.y; 268 | output->scale.z = mesh.scale.z; 269 | } 270 | output->mesh_resource = mesh.filename; 271 | // std::cout << mesh.filename << std::endl; 272 | } else if (link.visual->geometry->type == urdf::Geometry::SPHERE) { 273 | output->type = Marker::SPHERE; 274 | const urdf::Sphere& sphere = 275 | dynamic_cast(*link.visual->geometry); 276 | output->scale.x = 2 * sphere.radius; 277 | output->scale.y = 2 * sphere.radius; 278 | output->scale.z = 2 * sphere.radius; 279 | } 280 | } 281 | 282 | std::string Builder::NodeName(const std::string& name) { 283 | return name; 284 | } 285 | } // namespace robot_markers 286 | -------------------------------------------------------------------------------- /src/robot_markers/builder.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROBOT_MARKERS_BUILDER_H_ 2 | #define _ROBOT_MARKERS_BUILDER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "geometry_msgs/Pose.h" 8 | #include "ros/duration.h" 9 | #include "ros/time.h" 10 | #include "std_msgs/ColorRGBA.h" 11 | #include "transform_graph/transform_graph.h" 12 | #include "urdf/model.h" 13 | #include "visualization_msgs/Marker.h" 14 | #include "visualization_msgs/MarkerArray.h" 15 | 16 | #include "robot_markers/forward_kinematics.h" 17 | 18 | namespace robot_markers { 19 | /// \brief Builder creates a visualization of a robot, given its URDF model. 20 | /// 21 | /// You can either get the visualization as a visualization_msgs::MarkerArray or 22 | /// as a visualization_msgs::InteractiveMarker. 23 | /// The visualization is based on the URDF and should represent the robot with 24 | /// all joint positions set to 0. 25 | /// However, you can set the joint positions before building the marker. 26 | /// 27 | /// The API uses a builder pattern. You first create a builder with the URDF and 28 | /// initialize it (Init). Then, you set the namespace (the marker IDs 29 | /// start at 0 and count up), time stamp, frame ID, pose, color, etc. If no 30 | /// color is provided or the color values are all 0, the mesh color will be 31 | /// used. 32 | /// 33 | /// Once you have set all the marker fields, you then call Build to get the 34 | /// marker or markers representing the robot. 35 | /// 36 | /// \b Example: 37 | /// \code 38 | /// // Robot 1: Default configuration. 39 | /// visualization_msgs::MarkerArray robot1; 40 | /// builder.SetNamespace("robot"); 41 | /// builder.SetFrameId("base_link"); 42 | /// builder.SetTime(ros::Time::now()); 43 | /// builder.Build(&robot1); 44 | /// marker_arr_pub.publish(robot1); 45 | /// 46 | /// // Robot 2: Different pose and joints changed. 47 | /// visualization_msgs::MarkerArray robot2; 48 | /// builder.SetNamespace("robot2"); 49 | /// 50 | /// std::map joint_positions; 51 | /// joint_positions["torso_lift_joint"] = 0.1; 52 | /// joint_positions["head_tilt_joint"] = 0.5; 53 | /// builder.SetJointPositions(joint_positions); 54 | /// 55 | /// geometry_msgs::Pose pose; 56 | /// pose.position.y = 1; 57 | /// pose.orientation.w = 0.92387953; 58 | /// pose.orientation.z = -0.38268343; 59 | /// builder.SetPose(pose); 60 | /// builder.Build(&robot2); 61 | /// marker_arr_pub.publish(robot2); 62 | /// \endcode 63 | class Builder { 64 | public: 65 | /// Construct a Builder with the given URDF model. 66 | explicit Builder(const urdf::Model& model); 67 | 68 | /// \brief Initializes the Builder. 69 | /// 70 | /// Call Init() once before calling any other methods. 71 | /// This does NOT set any marker parameters (color, pose, etc.). 72 | void Init(); 73 | 74 | /// \brief Set the joint angles on the robot. 75 | /// 76 | /// If the robot does not have a joint, an error will be logged and the 77 | /// visualization will be unchanged. 78 | /// 79 | /// \param[in] joint_positions The joint angles/positions to set. This method 80 | /// does not check joint limits, so you must supply valid values. 81 | void SetJointPositions(const std::map joint_positions); 82 | 83 | /// \brief Set the output frame ID. 84 | /// 85 | /// All the markers will be given in this frame. 86 | /// 87 | /// \param[in] frame_id The frame ID for all the markers. 88 | void SetFrameId(const std::string& frame_id); 89 | 90 | /// \brief Set the time stamp. 91 | /// 92 | /// All the markers will be given this time stamp. 93 | /// 94 | /// \param[in] stamp The time stamp for all the markers. 95 | void SetTime(const ros::Time& stamp); 96 | 97 | /// \brief Set the marker namespace. 98 | /// 99 | /// Note that you cannot set the IDs of the markers. To visualize multiple 100 | /// robots, give each their own namespace. 101 | /// 102 | /// \param[in] ns The namespace for all the markers. 103 | void SetNamespace(const std::string& ns); 104 | 105 | /// \brief Set the pose of the robot, in the frame ID given by SetFrameId. 106 | /// 107 | /// \param[in] pose The pose of the robot. 108 | void SetPose(const geometry_msgs::Pose& pose); 109 | 110 | /// \brief Sets the color of the robot. 111 | /// 112 | /// Set to 0, 0, 0, 0 to use the mesh colors if available. If the mesh 113 | /// contains color information, then setting a non-zero color will tint the 114 | /// robot in that color. 115 | /// 116 | /// \param[in] r The red value, between 0 and 1. 117 | /// \param[in] g The green value, between 0 and 1. 118 | /// \param[in] b The blue value, between 0 and 1. 119 | /// \param[in] a The alpha value, between 0 and 1. 120 | void SetColor(float r, float g, float b, float a); 121 | 122 | /// \brief Sets the lifetime of the robot markers. 123 | /// 124 | /// \param[in] lifetime The lifetime for the robot markers. 125 | void SetLifetime(const ros::Duration& lifetime); 126 | 127 | /// \brief Sets whether the marker should be locked to its frame ID. 128 | /// 129 | /// The frame ID is supplied via SetFrameId. 130 | /// 131 | /// \param[in] frame_locked True to lock the markers to the robot's frame ID, 132 | /// false otherwise. 133 | void SetFrameLocked(bool frame_locked); 134 | 135 | /// \brief Builds a visualization of the robot model as a marker array. 136 | /// 137 | /// It sets marker properties according to the Set* methods in the class. 138 | /// You can build multiple sets of markers with the same builder, but you 139 | /// should be mindful of the history of property settings made beforehand. 140 | /// 141 | /// \param[out] marker_array The marker array message to append to. 142 | void Build(visualization_msgs::MarkerArray* marker_array, int no_color = 0); 143 | 144 | private: 145 | // Sets everything in the marker except the pose. 146 | void BuildMarker(const urdf::Link& link, int id, 147 | visualization_msgs::Marker* outpu, int no_color = 0); 148 | std::string NodeName(const std::string& name); 149 | 150 | urdf::Model model_; 151 | ForwardKinematics fk_; 152 | transform_graph::Graph tf_graph_; 153 | 154 | std::map joint_positions_; 155 | 156 | // Marker fields 157 | std::string frame_id_; 158 | ros::Time stamp_; 159 | std::string ns_; 160 | geometry_msgs::Pose pose_; 161 | std_msgs::ColorRGBA color_; 162 | ros::Duration lifetime_; 163 | bool frame_locked_; 164 | 165 | bool has_initialized_; 166 | }; 167 | } // namespace robot_markers 168 | 169 | #endif // _ROBOT_MARKERS_BUILDER_H_ 170 | -------------------------------------------------------------------------------- /src/robot_markers/forward_kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include "forward_kinematics.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "kdl/frames_io.hpp" 7 | #include "kdl/tree.hpp" 8 | #include "kdl_parser/kdl_parser.hpp" 9 | #include "ros/ros.h" 10 | #include "ros/time.h" 11 | #include "tf2_kdl/tf2_kdl.h" 12 | #include "urdf/model.h" 13 | 14 | namespace robot_markers { 15 | SegmentPair::SegmentPair(const KDL::Segment& p_segment, 16 | const std::string& p_root, const std::string& p_tip) 17 | : segment(p_segment), root(p_root), tip(p_tip) {} 18 | 19 | ForwardKinematics::ForwardKinematics(const urdf::Model& model) 20 | : model_(model), tree_(), segments_() {} 21 | 22 | void ForwardKinematics::Init() { 23 | kdl_parser::treeFromUrdfModel(model_, tree_); 24 | AddChildren(tree_.getRootSegment()); 25 | } 26 | 27 | void ForwardKinematics::GetTransforms( 28 | const std::map& joint_positions, 29 | std::vector* transforms) const { 30 | // Moving joints 31 | for (std::map::const_iterator jnt = 32 | joint_positions.begin(); 33 | jnt != joint_positions.end(); jnt++) { 34 | // ROS_INFO("Robot joint name \"%s\"", jnt->first.c_str()); 35 | std::map::const_iterator seg = 36 | segments_.find(jnt->first); 37 | if (seg != segments_.end()) { 38 | geometry_msgs::TransformStamped tf_transform = 39 | tf2::kdlToTransform(seg->second.segment.pose(jnt->second)); 40 | tf_transform.header.frame_id = seg->second.root; 41 | tf_transform.child_frame_id = seg->second.tip; 42 | 43 | 44 | // ROS_INFO("Robot header link name \"%s\"", seg->second.root.c_str()); 45 | // ROS_INFO("Robot child link name \"%s\"", seg->second.tip.c_str()); 46 | transforms->push_back(tf_transform); 47 | } else { 48 | ROS_ERROR("Robot does not have joint \"%s\"", jnt->first.c_str()); 49 | } 50 | } 51 | 52 | // Fixed joints. These are not needed for the purposes of drawing a marker 53 | // because the fixed joints are read by Builder::Init(). 54 | // However, this may be useful code to repurpose elsewhere. 55 | // 56 | // for (std::map::const_iterator seg = 57 | // segments_fixed_.begin(); 58 | // seg != segments_fixed_.end(); seg++) { 59 | // geometry_msgs::TransformStamped tf_transform = 60 | // tf2::kdlToTransform(seg->second.segment.pose(0)); 61 | // tf_transform.header.frame_id = seg->second.root; 62 | // tf_transform.child_frame_id = seg->second.tip; 63 | // transforms->push_back(tf_transform); 64 | //} 65 | } 66 | 67 | void ForwardKinematics::AddChildren( 68 | const KDL::SegmentMap::const_iterator segment) { 69 | const std::string& root = GetTreeElementSegment(segment->second).getName(); 70 | 71 | const std::vector& children = 72 | GetTreeElementChildren(segment->second); 73 | for (unsigned int i = 0; i < children.size(); i++) { 74 | const KDL::Segment& child = GetTreeElementSegment(children[i]->second); 75 | SegmentPair s(GetTreeElementSegment(children[i]->second), root, 76 | child.getName()); 77 | if (child.getJoint().getType() == KDL::Joint::None) { 78 | // if (model_.getJoint(child.getJoint().getName()) && 79 | // model_.getJoint(child.getJoint().getName())->type == 80 | // urdf::Joint::FLOATING) { 81 | //} else { 82 | // segments_fixed_.insert(make_pair(child.getJoint().getName(), s)); 83 | //} 84 | } else { 85 | segments_.insert(make_pair(child.getJoint().getName(), s)); 86 | } 87 | AddChildren(children[i]); 88 | } 89 | } 90 | } // namespace robot_markers 91 | -------------------------------------------------------------------------------- /src/robot_markers/forward_kinematics.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROBOT_MARKERS_FORWARD_KINEMATICS_H_ 2 | #define _ROBOT_MARKERS_FORWARD_KINEMATICS_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "geometry_msgs/TransformStamped.h" 8 | #include "kdl/frames.hpp" 9 | #include "kdl/segment.hpp" 10 | #include "kdl/tree.hpp" 11 | #include "urdf/model.h" 12 | 13 | namespace robot_markers { 14 | class SegmentPair { 15 | public: 16 | SegmentPair(const KDL::Segment& p_segment, const std::string& p_root, 17 | const std::string& p_tip); 18 | 19 | KDL::Segment segment; 20 | std::string root, tip; 21 | }; 22 | 23 | /// \brief Computes forward kinematics for the robot. 24 | /// 25 | /// Based on code from robot_state_publisher. 26 | class ForwardKinematics { 27 | public: 28 | /// Constructor. 29 | /// 30 | /// \param[in] model The URDF model. 31 | explicit ForwardKinematics(const urdf::Model& model); 32 | 33 | /// Initializes the forward kinematics. 34 | /// 35 | /// This should be called before calling any other methods. 36 | void Init(); 37 | 38 | /// Outputs the transforms for each set of joint angles passed in. 39 | /// 40 | /// If a joint is not part of the URDF, then it is skipped. 41 | /// 42 | /// \param[in] joint_positions The joint angles to query. 43 | /// \param[out] transforms The transforms, one for each joint angle. 44 | void GetTransforms( 45 | const std::map& joint_positions, 46 | std::vector* transforms) const; 47 | 48 | private: 49 | urdf::Model model_; 50 | KDL::Tree tree_; 51 | std::map segments_; 52 | 53 | void AddChildren(const KDL::SegmentMap::const_iterator segment); 54 | }; 55 | } // namespace robot_markers 56 | 57 | #endif // _ROBOT_MARKERS_FORWARD_KINEMATICS_H_ 58 | --------------------------------------------------------------------------------