Add can_id to speedCallback

- VESC now able to send messages with can_id
- Create new message VescCommand.msg usable by all callbacks

Author: sihengt

vesc_driver/include/vesc_driver/vesc_driver.h

@@ -37,6 +37,7 @@
 
 #include "vesc_driver/vesc_interface.h"
 #include "vesc_driver/vesc_packet.h"
+#include "vesc_msgs/VescCommand.h"
 
 namespace vesc_driver
 {
@@ -95,15 +96,12 @@ class VescDriver
   int fw_version_major_;                ///< firmware major version reported by vesc
   int fw_version_minor_;                ///< firmware minor version reported by vesc
 
-  // Optional
-  int can_id_ = -1;
-
   // ROS callbacks
   void timerCallback(const ros::TimerEvent& event);
   void dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle);
   void currentCallback(const std_msgs::Float64::ConstPtr& current);
   void brakeCallback(const std_msgs::Float64::ConstPtr& brake);
-  void speedCallback(const std_msgs::Float64::ConstPtr& speed);
+  void speedCallback(const vesc_msgs::VescCommand::ConstPtr& speed);
   void positionCallback(const std_msgs::Float64::ConstPtr& position);
   void servoCallback(const std_msgs::Float64::ConstPtr& servo);
 };

vesc_driver/launch/vesc_can.launch

@@ -0,0 +1,22 @@
+<!-- -*- mode: XML -*- -->
+<launch>
+
+  <arg name="node_name" default="vesc_driver_node" />
+
+  <!-- Optionally launch in GDB, for debugging -->
+  <arg name="debug" default="false" />
+
+  <arg if="$(arg debug)" name="launch_prefix" value="gdb -ex run --args" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+
+  <!-- VESC driver parameters -->
+  <arg name="port" default="/dev/serial/by-id/usb-STMicroelectronics_ChibiOS_RT_Virtual_COM_Port_304-if00" />
+
+  <!-- VESC driver node -->
+  <node pkg="vesc_driver" type="vesc_driver_node" name="$(arg node_name)"
+        output="screen" launch-prefix="$(arg launch_prefix)" >
+    <param name="port" value="$(arg port)" />
+    <param name="can_id" value="2" />
+  </node>
+
+</launch>

vesc_driver/src/vesc_driver.cpp

@@ -58,9 +58,6 @@ VescDriver::VescDriver(ros::NodeHandle nh,
     ros::shutdown();
     return;
   }
-
-  if (private_nh.getParam("can_id", can_id_))
-    ROS_INFO("Communicating with can_id = %d", can_id_);
 
   // attempt to connect to the serial port
   try
@@ -231,14 +228,18 @@ void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake)
  *              driver. However, note that the VESC may impose a more restrictive bounds on the
  *              range depending on its configuration.
  */
-void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed)
+void VescDriver::speedCallback(const vesc_msgs::VescCommand::ConstPtr& speed)
 {
+  ROS_DEBUG("Received speed callback.");
   if (driver_mode_ == MODE_OPERATING)
   {
-    if (can_id_ > 0)
-      vesc_.setSpeed(speed_limit_.clip(speed->data), can_id_);
+    if (speed->can_id > 0)
+    {
+      ROS_DEBUG("Setting speed with can_id");
+      vesc_.setSpeed(speed_limit_.clip(speed->command), speed->can_id);
+    }
     else
-      vesc_.setSpeed(speed_limit_.clip(speed->data));
+      vesc_.setSpeed(speed_limit_.clip(speed->command));
   }
 }
 

vesc_msgs/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(
   DIRECTORY msg
   FILES
+  VescCommand.msg
   VescState.msg
   VescStateStamped.msg
 )

vesc_msgs/msg/VescCommand.msg

@@ -0,0 +1,2 @@
+float32 command
+int32 can_id