Add ros rviz files for icub (#23)

robotology · Aug 19, 2019 · a5086bf · a5086bf
1 parent b69b989
commit a5086bf
Show file tree

Hide file tree

Showing 5 changed files with 219 additions and 0 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -20,6 +20,9 @@ endmacro()
 # Copy the iCub folder in the build tree
 file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/iCub DESTINATION ${CMAKE_CURRENT_BINARY_DIR})
 
+# Copy the ros folder
+file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/ros DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/iCub)
+
 # Add the model.config just for a limited number of models that are known
 # to work in Gazebo, unless the ICUB_MODELS_INSTALL_ALL_GAZEBO_MODELS option
 # is enabled

diff --git a/ros/README.md b/ros/README.md
@@ -0,0 +1,63 @@
+This directory contains the files related to [Rviz](http://wiki.ros.org/rviz)
+visualization of icub. The contents of this directory are installed inside `iCub`
+ros package. The location of `iCub` ros pacakge is discovered by running `roscd iCub`.
+The `launch` directory contains `roslaunch` file `iCubRviz.launch`. The robot model
+that will be used for rviz visualization depends `YARP_ROBOT_NAME` set in `.bashrc`.
+The default rviz configuration file is `iCubRviz.rviz` located inside `rviz` directory.
+The steps to run rviz visualization correctly are:
+
+- Ensure that the robot is started correctly with ros configuration files for motor control boards.
+  On the real robot, the [icub_ros_wrapper.xml](https://github.com/robotology/robots-configuration/blob/devel/iCubGenova04/wrappers/motorControl/icub_ros_wrapper.xml) configuration file must be included inside the startup
+  `xml` [configuration file](https://github.com/robotology/robots-configuration/blob/devel/iCubGenova04/icub_wbd.xml#L107).
+- On starting the robot, one should see `/<prefix>/joint_states` ros topic by running `rostopic list`.
+  Also, ensure that the `/<prefix>/joint_states` streams all the robot joint angles by running
+  `rostopic echo /<prefix>/joint_states`. The name of topic is set through [ROS_topicName](https://github.com/robotology/robots-configuration/blob/devel/iCubGenova04/wrappers/motorControl/icub_ros_wrapper.xml#L38)
+  parameter.
+- Start `transform server` by running `yarprobotinterface --config transform-server.xml`.
+  The transform server is a central location for transforms `tfs` and it streams the transforms
+  to `/tf` ros topic.
+- Launch [yarprobotstatepublisher](https://github.com/robotology/idyntree/tree/master/src/tools/yarprobotstatepublisher) that takes a tf-prefix, model, base frame name, joint states topic name as input and
+gives all the model transforms to the transform server. An example of running `yarprobotstatepublisher` is
+
+```
+yarprobotstatepublisher --name-prefix icub --tf-prefix /icub/ --model model.urdf --base-frame root_link --jointstates-topic "/icub04/joint_states"
+```
+ - Ensure that the transforms are available correctly by running `rostopic echo /tf`.
+   The transforms of the links are streamed with the prefix give to `--tf-prefix` of `yarprobotstatepublisher`.
+
+- Rviz visualization of icub is launched by running
+
+```
+  roslaunch iCub iCubRviz.launch
+```
+
+The following two parameters are important to ensure correct visualization:
+
+- Under `Global Options`, `Fixed Frame` field is the frame name with respect to
+  which all the other frames transforms are give. The default value set inside the `iCubRviz.rviz` configuration value is `icub/root_link`.
+
+  ![Selection_001](https://user-images.githubusercontent.com/6505998/62306298-e064e400-b481-11e9-9b5e-c665b5aaa5ff.png)
+
+
+- The next parameter is under `Robot Model`, `TF Prefix`. The default value set inside the `iCubRviz.rviz` configuration value is `icub`.
+  The value of the parameter should match the `--tf-prefix` of `yarprobotstatepublisher` for rviz to get the correct transforms to visualize the model.
+
+  ![Screenshot from 2019-08-01 11-37-26](https://user-images.githubusercontent.com/6505998/62283354-ab409d80-b451-11e9-9811-bd9158548e3e.png)
+
+The contents of this directory are tested on:
+
+##### Linux
+
+```
+Ubuntu 18.04 LTS
+Rosdistro: Melodic
+Rosversion: 1.14.3
+```
+
+##### MacOS
+
+```
+MacOs High Sierra 10.13.6
+Rosdistro: Lunar
+Rosversion: 1.13.7
+```
diff --git a/ros/launch/iCubRviz.launch b/ros/launch/iCubRviz.launch
@@ -0,0 +1,14 @@
+<launch>
+    <arg name="Robot"/>
+    <arg name="gui" default="False"/>
+    <arg name="vel" default="True"/>
+    <arg name="eff" default="True"/>
+    <arg name="YARP_ROBOT_NAME" default="$(env YARP_ROBOT_NAME)"/>
+    <arg name="robotModel" default="$(find iCub)/robots/$(arg YARP_ROBOT_NAME)/model.urdf  "/>
+    <arg name="rvizconfig" default="$(find iCub)/ros/rviz/iCubRviz.rviz"/>
+
+    <param name="robot_description" command="$(find xacro)/xacro $(arg robotModel)" />
+    <param name="use_gui" value="$(arg gui)"/>
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>
diff --git a/ros/rviz/iCubRviz.rviz b/ros/rviz/iCubRviz.rviz
@@ -0,0 +1,127 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 84
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 1043
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 173; 173; 177
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Billboards
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 1000
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: icub
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 255; 255; 255
+    Default Light: true
+    Fixed Frame: icub/root_link
+    Frame Rate: 60
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 7.744000434875488
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.16518345475196838
+        Y: 0.3169254660606384
+        Z: 0.5606259107589722
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.3903980255126953
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.770397961139679
+    Saved: ~
+Window Geometry:
+  "&Displays":
+    collapsed: false
+  "&Time":
+    collapsed: false
+  "&Views":
+    collapsed: false
+  Height: 768
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000004b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007601000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000004b4000000f701000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000004b4000000d001000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000004cfc0100000002fb0000000800540069006d0065010000000000000a00000002ad01000003fb0000000800540069006d0065010000000000000450000000000000000000000784000004b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Width: 1024
+  X: 2880
+  Y: 0
diff --git a/ros/transform-server.xml b/ros/transform-server.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<robot name="Transform-Server" build=0 portprefix="">
+
+    <device type="transformServer" name="TransformServer">
+        <param name="transforms_lifetime">0.2</param>
+        <group name="ROS">
+            <param name="enable_ros_publisher">true</param>
+            <param name="enable_ros_subscriber">false</param>
+        </group>
+    </device>
+
+</robot>