Read cartesian controller topic from rosparam

cfg/active_grasp.yaml

@@ -15,6 +15,9 @@ grasp_controller:
     depth_topic: /camera/depth/image_rect_raw
     min_z_dist: 0.3
 
+cartesian_velocity_controller:
+  topic: /cartesian_velocity_controller/set_command
+
 vgn:
   model: $(find vgn)/assets/models/vgn_conv.pth
   finger_depth: 0.05

launch/env.launch

@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="launch_rviz" default="false" />
   <arg name="simulation" default="true" />
+  <arg name="launch_rviz" default="false" />
 
   <!-- Load parameters -->
   <rosparam command="load" file="$(find active_grasp)/cfg/active_grasp.yaml" subst_value="true" />
@@ -9,6 +9,7 @@
   <!-- Load robot description -->
   <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/panda_arm_hand.urdf.xacro" />
 
+  <!-- Simulated environment -->
   <group if="$(arg simulation)">
     <node pkg="active_grasp" type="bt_sim_node.py" name="bt_sim" output="screen" />
     <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />

scripts/bt_sim_node.py

@@ -156,7 +156,8 @@ class CartesianVelocityControllerPlugin(Plugin):
         super().__init__(rate)
         self.arm = arm
         self.model = model
-        rospy.Subscriber("command", Twist, self.target_cb)
+        topic = rospy.get_param("cartesian_velocity_controller/topic")
+        rospy.Subscriber(topic, Twist, self.target_cb)
 
     def target_cb(self, msg):
         self.dx_d = from_twist_msg(msg)

src/active_grasp/controller.py

@@ -48,7 +48,8 @@ class GraspController:
     def init_robot_connection(self):
         self.arm = PandaArmClient()
         self.gripper = PandaGripperClient()
-        self.cartesian_vel_pub = rospy.Publisher("command", Twist, queue_size=10)
+        topic = rospy.get_param("cartesian_velocity_controller/topic")
+        self.cartesian_vel_pub = rospy.Publisher(topic, Twist, queue_size=10)
 
     def init_moveit(self):
         self.moveit = MoveItClient("panda_arm")