(5) Realsense camera integration

README.md

@@ -24,16 +24,19 @@ The table below lists the available parameters and their meaning.
 
 | Parameter name | Description |
 | -------------- | ----------- |
+| `robot_name` | Set the prefix name of the robot, 'tracebot' by default. |
+| `mount_base_width` | Width of the robot stand. |
+| `mount_base_length` | Length of the robot stand. |
+| `mount_base_height` | Height of the robot stand. |
 | `arm_mount_offset_x` | Offset in X direction of the robot arm with respect to the center of the stand. |
 | `arm_mount_offset_y` | Offset in Y direction of the robot arm with respect to the center of the stand. |
 | `arm_mount_offset_z` | Offset in Z direction of the robot arm with respect to the top of the stand. |
 | `arm_mount_offset_theta` | Rotational offset around Z of the robot arm with respect to the center of the stand. |
-| `mount_base_height` | Height of the robot stand. |
-| `mount_base_length` | Length of the robot stand. |
-| `mount_base_width` | Width of the robot stand. |
 | `arm_base_tilt` | Angular upward tilt of the robot bases. |
 | `left_arm_model` | Model used for left Robot Arm. Must use on of: (ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e) |
-| `right_arm_model` | Model used for right Robot Arm. Must use on of: (ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e) |
+| `right_arm_model` | Model used for right Robot Arm. Must use on of:  (ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e) |
+| `camera_mount_offset_z` | Offset in Z direction of the camera with respect to the top of the stand. |
+| `camera_tilt` | The forward tilt of the camera. |
 
 All parameters use SI units.
 
@@ -74,13 +77,14 @@ The pose of the camera in the world is defined within the launchfile [camera_dis
 
 | Parameter name | Description |
 | -------------- | ----------- |
+| `rviz` | `Launch rviz with gazebo (false by defalut).` |
 | `controller_config_file` | `Config file used for defining the ROS-Control controllers.` |
 | `controllers` | `Controllers that are activated by default.` |
 | `stopped_controllers` | `Controllers that are initally loaded, but not started.` |
 | `tf_prefix` | `tf_prefix used for the robot.` |
 | `tf_pub_rate` | `Rate at which robot_state_publisher should publish transforms.` |
-| `paused` | `Starts Gazebo in paused mode` |
-| `gui` | `Starts Gazebo gui` |
+| `paused` | `Starts Gazebo in paused mode.` |
+| `gui` | `Starts Gazebo gui.` |
 
 As shown above, the gazebo launch file has some additional parameters that can be set.
 However any of the previously stated parameters can also be used.
@@ -96,6 +100,7 @@ However any of the previously stated parameters can also be used.
 
 The arms can then be communicated with by publishing to the `/pos_joint_traj_controller/command` topic which uses [JointTrajectory messages](http://docs.ros.org/en/noetic/api/trajectory_msgs/html/msg/JointTrajectory.html).
 An example of this can be found in the [scripts/gazebo_model_test.py](tracebot_mockup_gazebo/scripts/gazebo_model_test.py) file.
+
 To run this, and test that the gazebo simulation is working, run:
 
 ```bash
@@ -104,6 +109,15 @@ rosrun tracebot_mockup_gazebo gazebo_model_test.py
 
 You should see that all the motors trigger and the arms curl up.
 
+Rviz can also be set to launch with the gazebo simulation using:
+```bash
+roslaunch tracebot_mockup_gazebo view_tracebot_gazebo.launch rviz:=true
+```
+If you wish to launch rviz seperately (e.g. if launching from a seperate device) there is also a launch file in the gazebo package for this:
+```bash
+roslaunch tracebot_mockup_gazebo view_tracebot_rviz.launch
+```
+
 ## Setup
 
 There's two different possibilities to use the packages in this repository: installing locally or running the pre-built docker images.
@@ -126,6 +140,7 @@ The examples listed below use [catkin_tools](https://catkin-tools.readthedocs.io
   cd ~/path/to/tracebot_mockup_ws/src
   git clone https://gitlab.com/tracebot/tracebot_mockup.git
   ```
+  #### Install Universal Arm package
 - Certain dependencies are not released as binary packages to either Melodic or Noetic, pull those into the workspace as well:
   ```bash
   cd ~/path/to/tracebot_mockup_ws/src
@@ -136,7 +151,18 @@ The examples listed below use [catkin_tools](https://catkin-tools.readthedocs.io
   cd ~/path/to/tracebot_mockup_ws
   rosdep install -iy --from-paths src --rosdistro "$ROS_DISTRO"
   ```
-- Build:
+  #### Install Realsense Camera packge
+- The realsense description package needs to be installed for the 3d model and urdf files using:
+  ```bash
+  sudo apt-get install ros-$ROS_DISTRO-realsense2-description
+  ```
+  more information about this package can be found ont the [realsense-ros github page](https://github.com/IntelRealSense/realsense-ros)
+
+- Then the realsense gazebo plugin needs to be installed, allowing the topics to be published (more information can be found on their [github page](https://github.com/pal-robotics/realsense_gazebo_plugin)):
+  ```bash
+  git clone https://github.com/pal-robotics/realsense_gazebo_plugin
+  ```
+- Then finally Build everything:
   ```bash
   cd ~/path/to/tracebot_mockup_ws
   catkin build

tracebot_mockup_description/config/tracebot_mockup.rviz

@@ -7,6 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
         - /TF1
+        - /TF1/Frames1
         - /Camera1
       Splitter Ratio: 0.5
     Tree Height: 1078
@@ -63,6 +64,28 @@ Visualization Manager:
           Value: false
         tracebot_base_link:
           Value: true
+        tracebot_camera_bottom_screw_frame:
+          Value: false
+        tracebot_camera_color_frame:
+          Value: false
+        tracebot_camera_color_optical_frame:
+          Value: false
+        tracebot_camera_depth_frame:
+          Value: false
+        tracebot_camera_depth_optical_frame:
+          Value: false
+        tracebot_camera_infra1_frame:
+          Value: false
+        tracebot_camera_infra1_optical_frame:
+          Value: false
+        tracebot_camera_infra2_frame:
+          Value: false
+        tracebot_camera_infra2_optical_frame:
+          Value: false
+        tracebot_camera_link:
+          Value: true
+        tracebot_camera_mount:
+          Value: false
         tracebot_left_arm_base:
           Value: false
         tracebot_left_arm_base_link:
@@ -120,6 +143,21 @@ Visualization Manager:
       Tree:
         tracebot_base_link:
           tracebot_arm_mount_base:
+            tracebot_camera_mount:
+              tracebot_camera_bottom_screw_frame:
+                tracebot_camera_link:
+                  tracebot_camera_color_frame:
+                    tracebot_camera_color_optical_frame:
+                      {}
+                  tracebot_camera_depth_frame:
+                    tracebot_camera_depth_optical_frame:
+                      {}
+                  tracebot_camera_infra1_frame:
+                    tracebot_camera_infra1_optical_frame:
+                      {}
+                  tracebot_camera_infra2_frame:
+                    tracebot_camera_infra2_optical_frame:
+                      {}
             tracebot_left_arm_mount:
               tracebot_left_arm_base_link:
                 tracebot_left_arm_base:
@@ -169,6 +207,51 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+        tracebot_camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tracebot_camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tracebot_camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tracebot_camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tracebot_camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tracebot_camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tracebot_camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tracebot_camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tracebot_camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tracebot_camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tracebot_camera_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         tracebot_left_arm_base:
           Alpha: 1
           Show Axes: false
@@ -329,7 +412,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 5.468780517578125
+      Distance: 3.4254446029663086
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -345,9 +428,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5353988409042358
+      Pitch: 0.5853990912437439
       Target Frame: <Fixed Frame>
-      Yaw: 0.5372345447540283
+      Yaw: 1.0722342729568481
     Saved: ~
 Window Geometry:
   Camera:

tracebot_mockup_description/launch/view_tracebot_mockup.launch

@@ -16,6 +16,9 @@
 
   <arg name="left_arm_model" default="ur5e"/>
   <arg name="right_arm_model" default="ur5e"/>
+
+  <arg name="camera_mount_offset_z" default="0.65169"/>
+  <arg name="camera_tilt" default="0.785398"/>
 
   <!--Load parameters into robot-->
   <param name="robot_description" command="$(find xacro)/xacro $(find tracebot_mockup_description)/urdf/tracebot_mockup.urdf.xacro
@@ -29,7 +32,9 @@
     arm_mount_offset_theta:=$(arg arm_mount_offset_theta)
     arm_base_tilt:=$(arg arm_base_tilt)
     left_arm_model:=$(arg left_arm_model)
-    right_arm_model:=$(arg right_arm_model)"
+    right_arm_model:=$(arg right_arm_model)
+    camera_mount_offset_z:=$(arg camera_mount_offset_z)
+    camera_tilt:=$(arg camera_tilt)"
   />
 
   <!--Load robot in Rviz-->

tracebot_mockup_description/urdf/tracebot_mockup.urdf.xacro

@@ -16,5 +16,6 @@
     arm_base_tilt="$(arg arm_base_tilt)"
     left_arm_model="$(arg left_arm_model)"
     right_arm_model="$(arg right_arm_model)"
-  />
+    camera_mount_offset_z="$(arg camera_mount_offset_z)"
+    camera_tilt="$(arg camera_tilt)"/>
 </robot>

tracebot_mockup_description/urdf/tracebot_mockup_macro.urdf.xacro

@@ -11,7 +11,9 @@
                                               arm_mount_offset_theta:=0
                                               arm_base_tilt:=0
                                               left_arm_model
-                                              right_arm_model">
+                                              right_arm_model
+                                              camera_mount_offset_z
+                                              camera_tilt">
     <!-- Base -->
     <link name="${robot_name}_base_link"/>
 
@@ -85,5 +87,24 @@
       safety_limits="false"
       safety_pos_margin="0.15"
       safety_k_position="20"/>
+
+    <!-- Camera Mount -->
+    <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
+
+    <joint name="${robot_name}_camera_mount_joint" type="fixed">
+      <origin xyz="0 0 ${mount_base_height+camera_mount_offset_z}" rpy="0 0 0"/>
+      <parent link="${robot_name}_arm_mount_base"/>
+      <child link="${robot_name}_camera_mount"/>
+    </joint>
+
+    <link name="${robot_name}_camera_mount"/>
+
+    <xacro:sensor_d435 parent="${robot_name}_camera_mount" 
+      use_nominal_extrinsics="true" 
+      add_plug="false" 
+      use_mesh="true"
+      name="${robot_name}_camera">
+      <origin xyz="0 0 0" rpy="0 ${camera_tilt} 1.5708"/>
+    </xacro:sensor_d435>
   </xacro:macro>
 </robot>