Finish adding the simulated realsense to the topbar, add support for …

…the physical realsense. Tidy up some parameters that were copied in last night but not yet configured.
fig-shimooka · Jun 3, 2020 · 6d39cb4 · 6d39cb4
1 parent 1bb2fd7
commit 6d39cb4
Show file tree

Hide file tree

Showing 8 changed files with 54 additions and 10 deletions.
diff --git a/husky_bringup/launch/realsense_config/realsense.launch b/husky_bringup/launch/realsense_config/realsense.launch
@@ -7,7 +7,7 @@
   <arg name="tf_prefix"           default="$(arg camera)"/>
   <arg name="external_manager"    default="false"/>
   <arg name="manager"             default="realsense2_camera_manager"/>
-  <arg name="base_frame_id"       default="$(optenv JACKAL_REANSENSE_MOUNT front_fender)_realsense" />
+  <arg name="base_frame_id"       default="sensor_arch_mount_link" />
 
   <arg name="fisheye_width"       default="640"/>
   <arg name="fisheye_height"      default="480"/>
@@ -111,4 +111,32 @@
     <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
     <arg name="allow_no_texture_points"  value="$(arg allow_no_texture_points)"/>
   </include>
+
+  <!-- Include poincloud_to_laserscan if realsense is attached -->
+  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="realsense_to_laserscan" output="screen">
+
+      <remap from="cloud_in" to="realsense/depth/color/points"/>
+      <remap from="scan" to="realsense/scan"/>
+      <!-- TODO: modify these to match the Realsense D400 series parameters -->
+      <rosparam>
+          target_frame: base_link # Leave empty to output scan in the pointcloud frame
+          tolerance: 1.0
+          min_height: 0.05
+          max_height: 1.0
+
+          angle_min: -0.7592182246175333 # -(87/2))*M_PI/180.0
+          angle_max: 0.7592182246175333 # (87/2))*M_PI/180.0
+          angle_increment: 0.005 # M_PI/360.0
+          scan_time: 0.3333
+          range_min: 0.105
+          range_max: 8.0
+          use_inf: true
+
+          # Concurrency level, affects number of pointclouds queued for processing and number of threads used
+          # 0 : Detect number of cores
+          # 1 : Single threaded
+          # 2->inf : Parallelism level
+          concurrency_level: 1
+      </rosparam>
+  </node>
 </launch>
diff --git a/husky_bringup/package.xml b/husky_bringup/package.xml
@@ -38,6 +38,7 @@
   <run_depend>tf2_ros</run_depend>
   <run_depend>um6</run_depend>
   <run_depend>um7</run_depend>
+  <run_depend>realsense2_camera</run_depend>
 
   <export>
   </export>

diff --git a/husky_control/launch/control.launch b/husky_control/launch/control.launch
@@ -10,6 +10,7 @@
 
   <arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED false)"/>
   <arg name="kinect_enabled" default="$(optenv HUSKY_KINECT_ENABLED false)"/>
+  <arg name="realsense_enabled" default="$(optenv HUSKY_REALSENSE_ENABLED false)"/>
   <arg name="urdf_extras" default="$(optenv HUSKY_URDF_EXTRAS)"/>
 
   <include file="$(find multimaster_launch)/launch/multimaster_robot.launch" if="$(arg multimaster)">
@@ -23,6 +24,7 @@
   <include file="$(find husky_description)/launch/description.launch" >
     <arg name="laser_enabled" default="$(arg laser_enabled)"/>
     <arg name="kinect_enabled" default="$(arg kinect_enabled)"/>
+    <arg name="realsense_enabled" default="$(arg realsense_enabled)"/>
     <arg name="urdf_extras" default="$(arg urdf_extras)"/>
   </include>
 

diff --git a/husky_description/urdf/husky.urdf.xacro b/husky_description/urdf/husky.urdf.xacro
@@ -169,9 +169,15 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 
   <xacro:if value="$(arg realsense_enabled)">
     <xacro:sensor_arch prefix="" parent="top_plate_link">
-      <origin xyz="-0.35 0 0.51" rpy="0 0 -3.14"/>
+      <origin xyz="-0.35 0 0.51" rpy="0 0 -3.14159"/>
     </xacro:sensor_arch>
-    <xacro:intel_realsense_mount prefix="camera" topic="realsense" parent_link="sensor_arch_mount_link"/>
+    <link name="realsense_mountpoint"/>
+    <joint name="realsense_mountpoint_joint" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 -3.14159" />
+      <parent link="sensor_arch_mount_link"/>
+      <child link="realsense_mountpoint" />
+    </joint>
+    <xacro:intel_realsense_mount prefix="camera" topic="realsense" parent_link="realsense_mountpoint"/>
   </xacro:if>
 
   <gazebo>

diff --git a/husky_gazebo/launch/husky_empty_world.launch b/husky_gazebo/launch/husky_empty_world.launch
@@ -29,6 +29,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 
   <arg name="laser_enabled" default="true"/>
   <arg name="kinect_enabled" default="false"/>
+  <arg name="realsense_enabled" default="false"/>
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(arg world_name)"/> <!-- world_name is wrt GAZEBO_RESOURCE_PATH environment variable -->
@@ -42,6 +43,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
   <include file="$(find husky_gazebo)/launch/spawn_husky.launch">
     <arg name="laser_enabled" value="$(arg laser_enabled)"/>
     <arg name="kinect_enabled" value="$(arg kinect_enabled)"/>
+    <arg name="realsense_enabled" value="$(arg realsense_enabled)"/>
   </include>
 
 </launch>
diff --git a/husky_gazebo/launch/husky_playpen.launch b/husky_gazebo/launch/husky_playpen.launch
@@ -27,12 +27,14 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 
   <arg name="laser_enabled" default="true"/>
   <arg name="kinect_enabled" default="false"/>
+  <arg name="realsense_enabled" default="false"/>
 
   <include file="$(find husky_gazebo)/launch/playpen.launch" />
 
   <include file="$(find husky_gazebo)/launch/spawn_husky.launch">
     <arg name="laser_enabled" value="$(arg laser_enabled)"/>
     <arg name="kinect_enabled" value="$(arg kinect_enabled)"/>
+    <arg name="realsense_enabled" value="$(arg realsense_enabled)"/>
   </include>
 
 </launch>
diff --git a/husky_gazebo/launch/realsense.launch b/husky_gazebo/launch/realsense.launch
@@ -1,8 +1,8 @@
 <launch>
   <!-- Include poincloud_to_laserscan if simulated realsense is attached -->
-  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan" output="screen">
+  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="realsense_to_laserscan" output="screen">
 
-      <remap from="cloud_in" to="camera/depth/points"/>
+      <remap from="cloud_in" to="realsense/depth/color/points"/>
       <remap from="scan" to="realsense/scan"/>
       <!-- TODO: modify these to match the Realsense D400 series parameters -->
       <rosparam>
@@ -11,12 +11,12 @@
           min_height: 0.05
           max_height: 1.0
 
-          angle_min: -0.52 # -30.0*M_PI/180.0
-          angle_max: 0.52 # 30.0*M_PI/180.0
+          angle_min: -0.7592182246175333 # -(87/2))*M_PI/180.0
+          angle_max: 0.7592182246175333 # (87/2))*M_PI/180.0
           angle_increment: 0.005 # M_PI/360.0
           scan_time: 0.3333
-          range_min: 0.45
-          range_max: 4.0
+          range_min: 0.105
+          range_max: 8.0
           use_inf: true
 
           # Concurrency level, affects number of pointclouds queued for processing and number of threads used

diff --git a/husky_gazebo/launch/spawn_husky.launch b/husky_gazebo/launch/spawn_husky.launch
@@ -35,7 +35,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
   <arg name="yaw" default="0.0"/>
 
   <arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED false)"/>
-  <arg name="kinect_enabled" default="$(optenv HUSKY_UR5_ENABLED false)"/>
+  <arg name="kinect_enabled" default="$(optenv HUSKY_KINECT_ENABLED false)"/>
+  <arg name="realsense_enabled" default="$(optenv HUSKY_REALSENSE_ENABLED false)"/>
   <arg name="urdf_extras" default="$(optenv HUSKY_URDF_EXTRAS)"/>
 
   <group ns="$(arg robot_namespace)">
@@ -45,6 +46,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
         <arg name="robot_namespace" value="$(arg robot_namespace)"/>
         <arg name="laser_enabled" default="$(arg laser_enabled)"/>
         <arg name="kinect_enabled" default="$(arg kinect_enabled)"/>
+        <arg name="realsense_enabled" default="$(arg realsense_enabled)"/>
         <arg name="urdf_extras" default="$(arg urdf_extras)"/>
       </include>
 
@@ -64,6 +66,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
         <arg name="multimaster" value="$(arg multimaster)"/>
         <arg name="laser_enabled" value="$(arg laser_enabled)"/>
         <arg name="kinect_enabled" value="$(arg kinect_enabled)"/>
+        <arg name="realsense_enabled" default="$(arg realsense_enabled)"/>
         <arg name="urdf_extras" value="$(arg urdf_extras)"/>
       </include>
     </group>