Merge pull request #61 from smarc-project/dr_sim

Updated sim to use DVL as twist and made installable
smarc-project · Apr 28, 2021 · 38b3bd8 · 38b3bd8
2 parents 004a794 + 605a32f
commit 38b3bd8
Show file tree

Hide file tree

Showing 7 changed files with 136 additions and 86 deletions.
diff --git a/release_packages.yaml b/release_packages.yaml
@@ -1,2 +1,3 @@
 - tf_lat_lon
 - tf_convenience_topics
+- sam_dead_reckoning
diff --git a/sam_dead_reckoning/CMakeLists.txt b/sam_dead_reckoning/CMakeLists.txt
@@ -176,6 +176,9 @@ target_link_libraries(dr_node
 
 # all install targets should use catkin DESTINATION variables
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+catkin_install_python(PROGRAMS scripts/acc_model.py scripts/press_to_depth.py scripts/dvl_twist.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
@@ -185,11 +188,11 @@ target_link_libraries(dr_node
 # )
 
 ## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+install(TARGETS dr_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark cpp header files for installation
 # install(DIRECTORY include/${PROJECT_NAME}/
@@ -204,6 +207,15 @@ target_link_libraries(dr_node
 #   # myfile2
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY params/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/params
+  PATTERN ".svn" EXCLUDE
+)
 
 #############
 ## Testing ##

diff --git a/sam_dead_reckoning/launch/dual_ekf_test.launch b/sam_dead_reckoning/launch/dual_ekf_test.launch
@@ -14,54 +14,61 @@
 
   <arg name="map_odom_tf_ekf" default="false"/>
 
-  <group ns="$(arg robot_name)">
-
-        <group ns="dr">
-
-    <include file="$(find tf_lat_lon)/launch/tf_lat_lon.launch">
-        <arg name="frame" value="$(arg robot_name)/base_link"/>
-    </include>
-
-    <!-- If EKF set to not bc this transform, do it here -->
-    <!--Publish a static coordinate transform to tf using offsets passed as arguments x/y/z(0/0/0 in meters) and yaw/pitch/roll(0/0/0 in radians)-->
-    <!--100 ms is the period, which specifies how often to send the transform-->
-    <node name="map_2_odom" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 map $(arg robot_name)/odom 100" unless="$(arg map_odom_tf_ekf)"/>
-
-<!--   <node pkg="sam_dead_reckoning" name="motion_model" type="motion_model.py" output="screen">
-    <param name="thrust_dr" value="/$(arg robot_name)/dr/motion_dr" />
-    <param name="thrust_fb" value="/$(arg robot_name)/core/rpm_fb" />
-    <param name="sbg_imu" value="/$(arg robot_name)/core/sbg_imu" />
-    <param name="odom_frame" value="$(arg robot_name)/odom" />
-    <param name="base_frame" value="$(arg robot_name)/base_link" />
-  </node> -->
-
-  <node pkg="sam_dead_reckoning" name="acc_model" type="acc_model.py" output="screen">
-    <param name="thrust1_fb" value="/$(arg robot_name)/core/thruster1_fb" />
-    <param name="thrust2_fb" value="/$(arg robot_name)/core/thruster2_fb" />
-    <param name="thrust_acc" value="/$(arg robot_name)/dr/motion_acc" />
-  </node>
-
-  <!-- Depth from pressure sensor converter -->
-  <node pkg="sam_dead_reckoning" name="press_to_depth" type="press_to_depth.py" output="screen">
-    <param name="odom_frame" value="$(arg robot_name)/odom" />
-    <param name="pressure_topic" value="/$(arg robot_name)/core/depth20_pressure" />
-    <param name="depth_topic" value="pressure_depth"/>                               <!---->
-    <param name="pressure_frame" value="$(arg robot_name)/pressure_link" />
-    <param name="base_frame" value="$(arg robot_name)/base_link" />
-  </node>
-
-  <!-- DVL to Twist msg converter -->
-  <node pkg="sam_dead_reckoning" name="dvl_dr" type="dvl_dr.py" output="screen">
-    <param name="dvl_topic" value="/$(arg robot_name)/core/dvl" />
-    <!-- <param name="dvl_twist_topic" value="dvl_twist"/> -->
-    <param name="dvl_dr_topic" value="/$(arg robot_name)/dr/dvl_dr" />
-    <param name="sbg_imu" value="/$(arg robot_name)/core/sbg_imu" />
-    <param name="odom_frame" value="$(arg robot_name)/odom" />
-    <param name="base_frame" value="$(arg robot_name)/base_link" />
-    <param name="dvl_link" value="$(arg robot_name)/dvl_link" />
-    <param name="dr_odom_filtered" value="local/odom/filtered" />
-  </node>
+    <group ns="$(arg robot_name)">
+    <group ns="dr">
 
+        <include file="$(find tf_lat_lon)/launch/tf_lat_lon.launch">
+            <arg name="frame" value="$(arg robot_name)/base_link"/>
+        </include>
+
+        <!-- If EKF set to not bc this transform, do it here -->
+        <!--Publish a static coordinate transform to tf using offsets passed as arguments x/y/z(0/0/0 in meters) and yaw/pitch/roll(0/0/0 in radians)-->
+        <!--100 ms is the period, which specifies how often to send the transform-->
+        <node name="map_2_odom" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 map $(arg robot_name)/odom 100" unless="$(arg map_odom_tf_ekf)"/>
+
+    <!--   <node pkg="sam_dead_reckoning" name="motion_model" type="motion_model.py" output="screen">
+        <param name="thrust_dr" value="/$(arg robot_name)/dr/motion_dr" />
+        <param name="thrust_fb" value="/$(arg robot_name)/core/rpm_fb" />
+        <param name="sbg_imu" value="/$(arg robot_name)/core/sbg_imu" />
+        <param name="odom_frame" value="$(arg robot_name)/odom" />
+        <param name="base_frame" value="$(arg robot_name)/base_link" />
+      </node> -->
+
+      <node pkg="sam_dead_reckoning" name="acc_model" type="acc_model.py" output="screen">
+        <param name="thrust1_fb" value="/$(arg robot_name)/core/thruster1_fb" />
+        <param name="thrust2_fb" value="/$(arg robot_name)/core/thruster2_fb" />
+        <param name="thrust_acc" value="/$(arg robot_name)/dr/motion_acc" />
+      </node>
+
+      <!-- Depth from pressure sensor converter -->
+      <node pkg="sam_dead_reckoning" name="press_to_depth" type="press_to_depth.py" output="screen">
+        <param name="odom_frame" value="$(arg robot_name)/odom" />
+        <param name="pressure_topic" value="/$(arg robot_name)/core/pressure" />
+        <param name="depth_topic" value="pressure_depth"/>                               <!---->
+        <param name="pressure_frame" value="$(arg robot_name)/pressure_link" />
+        <param name="base_frame" value="$(arg robot_name)/base_link" />
+      </node>
+
+      <!-- DVL to Twist msg converter -->
+      <node pkg="sam_dead_reckoning" name="dvl_twist" type="dvl_twist.py" output="screen">
+        <param name="dvl_topic" value="/$(arg robot_name)/core/dvl" />
+        <param name="twist_topic" value="/$(arg robot_name)/dr/dvl_twist" />
+        <param name="dvl_link" value="$(arg robot_name)/dvl_link" />
+      </node>
+      <!--
+      <node pkg="sam_dead_reckoning" name="dvl_dr" type="dvl_dr.py" output="screen">
+        <param name="dvl_topic" value="/$(arg robot_name)/core/dvl" />
+        <param name="dvl_dr_topic" value="/$(arg robot_name)/dr/dvl_dr" />
+        <param name="sbg_imu" value="/$(arg robot_name)/core/sbg_imu" />
+        <param name="odom_frame" value="$(arg robot_name)/odom" />
+        <param name="base_frame" value="$(arg robot_name)/base_link" />
+        <param name="dvl_link" value="$(arg robot_name)/dvl_link" />
+        <param name="dr_odom_filtered" value="local/odom/filtered" />
+      </node>
+      -->
+
+<!-- NOTE: this should be part of the drivers instead! -->
+<!--
 	<node pkg="sam_dead_reckoning" name="sbg_2_ros" type="sbg_to_ros.py" output="screen">
 		<param name="sbg_imu_data" value="/$(arg robot_name)/sbg/imu_data"/>
 		<param name="sbg_ekf_quat" value="/$(arg robot_name)/sbg/ekf_quat"/>
@@ -70,8 +77,7 @@
 		<param name="sbg_frame" value="$(arg robot_name)/sbg_link"/>
 		<param name="sbg_imu_out" value="/$(arg robot_name)/core/sbg_imu"/>
 	</node>
-
-
+-->
 
   <!--The rosparam tag enables the use of rosparam YAML files(params/dual_ekf_sam.yaml in this case) for dumping parameters 
   from the ROS parameter server which the Nodes uses at runtime to store and retrieve parameters-->
@@ -81,13 +87,13 @@
         and produces an odometry message in coordinates that are consistent with  robot’s world frame.-->
 	<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" clear_params="true">
 
-    <!-- remap from= "/different_topic topic" to="/needed_topic" -->
-    <!--Now, when this node subscribes to topic /different_topic, the remapping makes it actually subscribe to topic /needed_topic.
-     So, anyone publishing to /needed_topic ends up getting their message to this new navsat_transform_node as well!-->
-
-  	<remap from="imu/data" to="/$(arg robot_name)/core/sbg_imu" />              <!--message with orientation data-->
-   	<remap from="gps/fix" to="/$(arg robot_name)/core/gps" />                   <!--message containing your robot’s GPS coordinates-->
-    <remap from="odometry/filtered" to="global/odom/filtered" />                <!--message of your robot’s current position-->
+        <!-- remap from= "/different_topic topic" to="/needed_topic" -->
+        <!--Now, when this node subscribes to topic /different_topic, the remapping makes it actually subscribe to topic /needed_topic.
+         So, anyone publishing to /needed_topic ends up getting their message to this new navsat_transform_node as well!-->
+         
+        <remap from="imu/data" to="/$(arg robot_name)/core/sbg_imu" />              <!--message with orientation data-->
+        <remap from="gps/fix" to="/$(arg robot_name)/core/gps" />                   <!--message containing your robot’s GPS coordinates-->
+        <remap from="odometry/filtered" to="global/odom/filtered" />                <!--message of your robot’s current position-->
 	</node>
 
 
@@ -121,7 +127,7 @@
                                          0.0,  0.0,  0.0,
                                          0.0,  0.0,  0.0]</rosparam>
 
- <!-- Depth sensor -->
+        <!-- Depth sensor -->
         <param name="pose0" value="pressure_depth"/>                             <!--topic name for Depth sensor : pressure_depth-->
         <!--only the z variable of the pose message is fused into the final state estimate-->
         <rosparam param="pose0_config">[false, false, true,
@@ -143,19 +149,18 @@
                                            false, false, false]</rosparam>
         <param name="odom0_differential" value="false"/> -->
 
-  <!-- DVL -->
-        <param name="odom0" value="dvl_dr"/>                                     <!--topic name for DVL sensor : dvl_dr-->
-        <!--variable x and y of the odometry message from the DVL sensor are fused in the final state estimate-->
-        <rosparam param="odom0_config">[true, true, false,
-                                        false, false, false,
+        <!-- DVL -->
+        <param name="twist0" value="dvl_twist"/>
+        <rosparam param="twist0_config">[false, false, false,
                                         false, false, false,
+                                        true, true, true,
                                         false, false, false,
                                         false, false, false]</rosparam>
 
-        <param name="odom0_differential" value="false"/>
-        <param name="odom0_relative" value="false"/>
+        <param name="twist0_differential" value="false"/>
+        <param name="twist0_relative" value="true"/> 
 
-	<!-- GPS -->
+        <!-- GPS -->
     	<!-- <param name="odom1" value="/gps_odom"/> -->
     	<param name="odom1" value="odometry/gps"/>                                  <!--topic name for GPS sensor : odometry/gps-->
       <!--variable x and y of the odometry message from the GPS sensor are fused in the final state estimate-->
@@ -166,7 +171,7 @@
                                    false, false, false]</rosparam>
     	<param name="odom1_differential" value="false"/>
 
-	<!-- SBG IMU -->
+        <!-- SBG IMU -->
         <param name="imu0" value="/$(arg robot_name)/core/sbg_imu"/>              <!--topic name for SBG IMU sensor : sam/core/sbg_imu-->
         <!--variable yaw and yaw_velocity are fused to final state estimate-->
         <rosparam param="imu0_config">[false, false, false,
@@ -184,7 +189,7 @@
 
 
   <!-- STIM IMU -->
-        <param name="imu1" value="/$(arg robot_name)/core/stim_imu"/>             <!--topic name for STIM IMU sensor : sam/core/stim_imu-->
+        <param name="imu1" value="/$(arg robot_name)/core/imu"/>             <!--topic name for STIM IMU sensor : sam/core/stim_imu-->
         <!--variables roll, pitch, roll_velocity, pitch_velocity of the imu message from the sensor are fused-->
         <rosparam param="imu1_config">[false, false, false,                       
                                        true, true, false,
@@ -227,13 +232,14 @@
                                                                                     So, anyone publishing to /motion_acc ends up getting their message to this new ekf_localization_node as well!-->
 
         <!-- Outputs -->
-        <remap from="odometry/filtered" to="local/odom/filtered"/>
+        <!-- <remap from="odometry/filtered" to="local/odom/filtered"/> -->
+        <remap from="odometry/filtered" to="odom"/>
         <remap from="set_pose" to="/initialpose"/>
     </node>
 
 
 	<!-- EKF for fusing GPS and continuous estimate -->
-   <!-- name : ekf_loc_global from the dual_ekf_test.yaml file-->
+    <!-- name : ekf_loc_global from the dual_ekf_test.yaml file-->
     <node pkg="robot_localization" type="ekf_localization_node" name="ekf_loc_global" output="screen" clear_params="true">
         <param name="frequency" value="100"/>
         <param name="sensor_timeout" value="0.1"/>
@@ -295,15 +301,15 @@
 
 
         <!-- DVL -->
-        <param name="odom1" value="dvl_dr"/>
-        <rosparam param="odom1_config">[true, true, false,
-                                        false, false, false,
+        <param name="twist0" value="dvl_twist"/>
+        <rosparam param="twist0_config">[false, false, false,
                                         false, false, false,
+                                        true, true, true,
                                         false, false, false,
                                         false, false, false]</rosparam>
 
-        <param name="odom1_differential" value="false"/>
-        <param name="odom1_relative" value="false"/> 
+        <param name="twist0_differential" value="false"/>
+        <param name="twist0_relative" value="true"/> 
 
         <!-- SBG IMU -->
         <param name="imu0" value="/$(arg robot_name)/core/sbg_imu"/>
@@ -321,7 +327,7 @@
         <param name="gravitational_acceleration" value="9.80665"/>
 
         <!-- STIM IMU -->
-        <param name="imu1" value="/$(arg robot_name)/core/stim_imu"/>
+        <param name="imu1" value="/$(arg robot_name)/core/imu"/>
         <rosparam param="imu1_config">[false, false, false,
                                        false, false, false,
                                        false, false, false,
@@ -367,8 +373,7 @@
         	<param name="loop_freq" value="10" />
     	</node>
 
-        </group>
-
-  </group>
+    </group>
+    </group>
 
 </launch>
diff --git a/sam_dead_reckoning/package.xml b/sam_dead_reckoning/package.xml
@@ -61,12 +61,14 @@
   <build_depend>std_msgs</build_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <exec_depend>std_msgs</exec_depend>
-  <depend>sbg_driver</depend>
+  <!-- <depend>sbg_driver</depend> -->
   <depend>sensor_msgs</depend>
   <depend>smarc_msgs</depend>
   <depend>nav_msgs</depend>
 
+  <exec_depend>tf</exec_depend>
   <exec_depend>tf_convenience_topics</exec_depend>
+  <exec_depend>robot_localization</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

diff --git a/sam_dead_reckoning/scripts/acc_model.py b/sam_dead_reckoning/scripts/acc_model.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/env python
 
 import rospy
 import numpy as np

diff --git a/sam_dead_reckoning/scripts/dvl_twist.py b/sam_dead_reckoning/scripts/dvl_twist.py
@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+import rospy
+from smarc_msgs.msg import DVL
+from geometry_msgs.msg import TwistWithCovarianceStamped
+
+class DVLTwist(object):
+
+    def __init__(self):
+        self.dvl_topic = rospy.get_param('~dvl_topic', '/sam/core/dvl')
+        self.twist_topic = rospy.get_param('~twist_topic', 'dvl_twist')
+        self.dvl_frame = rospy.get_param('~dvl_link', 'sam/dvl_link')
+        self.sub = rospy.Subscriber(self.dvl_topic, DVL, self.dvl_callback)
+        self.pub = rospy.Publisher(self.twist_topic, TwistWithCovarianceStamped, queue_size=10)
+
+    def dvl_callback(self, dvl_msg):
+
+        twist_msg = TwistWithCovarianceStamped()
+        twist_msg.header = dvl_msg.header
+        twist_msg.twist.twist.linear = dvl_msg.velocity
+        for i in range(0, 3):
+            for j in range(0, 3):
+                twist_msg.twist.covariance[6*i+j] = dvl_msg.velocity_covariance[3*i+j]
+
+        self.pub.publish(twist_msg)
+
+if __name__ == "__main__":
+    rospy.init_node('dvl_twist')
+    dvl_twist = DVLTwist()
+    rospy.spin()
diff --git a/sam_dead_reckoning/scripts/press_to_depth.py b/sam_dead_reckoning/scripts/press_to_depth.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/env python
 
 import rospy
 import numpy as np