add remaps, fix renaming, add timeout param

auv_control/auv_control/launch/start.launch

@@ -4,6 +4,7 @@
   <arg name="rate" default="20" />
   <arg name="use_gui" default="false" />
   <arg name="start_state_publisher" default="false" />
+  <arg name="command_timeout" default="0.1" />
 
   <include file="$(find auv_description)/launch/start_state_publisher.launch"
     if="$(arg start_state_publisher)">
@@ -33,9 +34,16 @@
       <rosparam file="$(find auv_control)/config/default.yaml" command="load" ns="" />
     </node>
 
-    <node pkg="auv_control" type="depth_controller_node.py" name="depth_controller_node"
+    <node pkg="auv_control" type="reference_pose_publisher.py" name="reference_pose_publisher_node"
       output="screen">
       <param name="update_rate" value="$(arg rate)" />
+      <param name="command_timeout" value="$(arg command_timeout)" />
+
+      <remap from="odometry" to="odometry" />
+      <remap from="set_depth" to="set_depth" />
+      <remap from="cmd_vel" to="cmd_vel" />
+      <remap from="cmd_pose" to="cmd_pose" />
+      <remap from="enable" to="enable" />
     </node>
 
 

auv_control/auv_control/scripts/reference_pose_publisher.py

@@ -8,23 +8,19 @@
 from auv_common_lib.control.enable_state import ControlEnableHandler
 
 
-class DepthControllerNode:
+class ReferencePosePublisherNode:
     def __init__(self):
-        rospy.init_node("reference_pose_publisher_node")
-
         # Initialize subscribers
         self.odometry_sub = rospy.Subscriber(
-            "/taluy/odometry", Odometry, self.odometry_callback
+            "odometry", Odometry, self.odometry_callback
         )
         self.set_depth_service = rospy.Service(
-            "/taluy/set_depth", SetDepth, self.target_depth_handler
-        )
-        self.cmd_vel_sub = rospy.Subscriber(
-            "/taluy/cmd_vel", Twist, self.cmd_vel_callback
+            "set_depth", SetDepth, self.target_depth_handler
         )
+        self.cmd_vel_sub = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
 
         # Initialize publisher
-        self.cmd_pose_pub = rospy.Publisher("/taluy/cmd_pose", Pose, queue_size=10)
+        self.cmd_pose_pub = rospy.Publisher("cmd_pose", Pose, queue_size=10)
 
         self.control_enable_handler = ControlEnableHandler(1.0)
 
@@ -35,6 +31,7 @@ def __init__(self):
 
         # Parameters
         self.update_rate = rospy.get_param("~update_rate", 10)
+        self.command_timeout = rospy.get_param("~command_timeout", 0.1)
 
         rospy.Timer(rospy.Duration(1.0 / self.update_rate), self.control_loop)
 
@@ -61,7 +58,7 @@ def cmd_vel_callback(self, msg):
             return
 
         dt = (rospy.Time.now() - self.last_cmd_time).to_sec()
-        dt = min(dt, 0.1)
+        dt = min(dt, self.command_timeout)
 
         self.target_heading += msg.angular.z * dt
         self.last_cmd_time = rospy.Time.now()
@@ -86,7 +83,8 @@ def run(self):
 
 if __name__ == "__main__":
     try:
-        depth_controller_node = DepthControllerNode()
-        depth_controller_node.run()
+        rospy.init_node("reference_pose_publisher_node")
+        reference_pose_publisher_node = ReferencePosePublisherNode()
+        reference_pose_publisher_node.run()
     except rospy.ROSInterruptException:
         pass