Motor topic refactor

autonomy/src/autonomy/automodule/cmd_to_wheel_speeds.py

@@ -3,7 +3,7 @@
 
 import rospy
 from geometry_msgs.msg import Twist
-from hwctrl.msg import MotorCmd, DriveMotorCmd
+from hwctrl.msg import MotorCmd
 
 # Make these members of class later
 failed = False
@@ -15,6 +15,12 @@
 
 
 class CmdToWheelSpeedsNode:
+    """
+    Most ros nodes publish robot command velocities as a Twist message
+    The hwctrl expects MotorCmd messages (units of RPM in this case)
+    This node does that conversion. 
+    This could be factored out to the hwctrl if needed
+    """
     def __init__(self):
         rospy.init_node("cmd_to_wheel_speeds_node", anonymous=False)
 
@@ -24,7 +30,9 @@ def __init__(self):
 
         self.motor_acceleration = rospy.get_param('/motor_command_accel')
 
-        self.motor_setpoint_pub = rospy.Publisher("/glenn_base/motor_cmds", DriveMotorCmd, queue_size=2)
+        self.left_motor_pub = rospy.Publisher("/glenn_base/port_motor_cmd", MotorCmd, queue_size=2)
+        self.right_motor_pub = rospy.Publisher("/glenn_base/starboard_motor_cmd", MotorCmd, queue_size=2)
+
         rospy.Subscriber("/glenn_base/cmd_vel", Twist, self.cmd_vel_callback, queue_size=1)
 
         rospy.spin()
@@ -38,4 +46,6 @@ def cmd_vel_callback(self, msg):
 
         right_cmd = MotorCmd(setpoint=right_speed, acceleration=self.motor_acceleration)
         left_cmd = MotorCmd(setpoint=left_speed, acceleration=self.motor_acceleration)
-        self.motor_setpoint_pub.publish(right=right_cmd, left=left_cmd)
+
+        self.left_motor_pub.publish(left_cmd)
+        self.right_motor_pub.publish(right_cmd)

glenn_launcher/params/hardware/hwctrl_params.yaml

@@ -95,6 +95,7 @@ hardware:
     port_drive:
       id: 5
       name: 'PORT DRIVE'
+      topic: '/glenn_base/port_motor_cmd'
       type: 'vesc'
       can_id: 5
       gear_reduction: 630 # gear_reduc*pole_pairs
@@ -104,6 +105,7 @@ hardware:
     starboard_drive:
       id: 6
       name: 'STARBOARD DRIVE'
+      topic: '/glenn_base/starboard_motor_cmd'
       type: 'vesc'
       can_id: 6
       gear_reduction: 630 # gear_reduc*pole_pairs
@@ -114,6 +116,7 @@ hardware:
       id: 7
       name: 'DEPOSITION DUNKERINO'
       type: 'vesc'
+      topic: "/deposition/winch_cmd"
       can_id: 7
       gear_reduction: 245 # gear_reduc*pole_pairs
       max_rpm: 140
@@ -122,6 +125,7 @@ hardware:
     exc_belt:
       id: 8
       name: 'EXCAVATION BELT'
+      topic: "/excavation/conveyor_cmd"
       type: 'vesc'
       can_id: 8
       gear_reduction: 245 # gear_reduc*pole_pairs
@@ -131,6 +135,7 @@ hardware:
     exc_translation:
       id: 9
       name: 'EXCAVATION TRANSLATION'
+      topic: "/excavation/depth_cmd"
       type: 'vesc'
       can_id: 9
       gear_reduction: 245 # gear_reduc*pole_pairs
@@ -140,10 +145,12 @@ hardware:
     exc_port_act:
       id: 10
       name: 'EXCAVATION PORT ACTUATOR'
+      topic: "/excavation/angle_cmd"
       type: 'brushed_motor'
       can_id: 10
     exc_starboard_act:
       id: 11
       name: 'EXCAVATION STARBOARD ACTUATOR'
+      topic: "/excavation/angle_cmd"
       type: 'brushed_motor'
       can_id: 11

hwctrl/CMakeLists.txt

@@ -14,7 +14,7 @@ endif()
 if(CMAKE_BUILD_TYPE STREQUAL "Release")
     add_compile_options(-std=c++14 -O2)
 else()
-    add_compile_options(-std=c++14 -Wall -Wextra -pedantic -g)
+    add_compile_options(-std=c++14 -Wall) # -Wextra -pedantic -g)
 endif()
 
 set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE)

hwctrl/compile_commands.json

@@ -0,0 +1 @@
+../../../build/hwctrl/compile_commands.json

hwctrl/include/hardware/bmc.h

@@ -6,12 +6,7 @@
 
 class BMCMotor : CanMotor {
  public:
-  BMCMotor(ros::NodeHandle nh, const std::string& name, uint32_t id,
-           uint32_t can_id,
-           ros::Duration update_pd = ros::Duration(MOTOR_LOOP_PERIOD),
-           float accel_setpoint = DEFAULT_MAX_ACCEL,
-           float max_accel = DEFAULT_MAX_ACCEL, float max_rpm = DEFAULT_MAX_RPM,
-           float gear_reduc = 1.0f, ros::Duration timeout = ros::Duration(2.0));
+  BMCMotor(ros::NodeHandle nh, MotorConfig const& config);
   virtual ~BMCMotor() = default;
 
   virtual void setup() override final;