Ported the ROS2 fox drivers to ROS1 Noetic

.gitignore

@@ -1 +1,2 @@
 *.swp
+.vscode/

README.md

@@ -1,5 +1,8 @@
 # Veddar VESC Interface
+# ROS1 VESC Package
 
 ![ROS1 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS1%20CI%20Workflow/badge.svg)
 
 Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details
+
+The ```noetic``` branch contains the ROS2 branch back ported to ROS1 such that it is compatible with VESC (tested on the VESC EDU)

vesc.yaml

@@ -0,0 +1,42 @@
+
+# erpm (electrical rpm) = speed_to_erpm_gain * speed (meters / second) + speed_to_erpm_offset
+speed_to_erpm_gain: 5420 #4614
+speed_to_erpm_offset: 0.0
+
+# Set gains for converting acceleration to current and brake control values
+accel_to_current_gain: 100
+accel_to_brake_gain: -80
+
+tachometer_ticks_to_meters_gain: 0.00225
+# servo smoother - limits rotation speed and smooths anything above limit
+max_servo_speed: 3.2 # radians/second
+servo_smoother_rate: 75.0 # messages/sec
+
+# servo smoother - limits acceleration and smooths anything above limit
+max_acceleration: 2.5 # meters/second^2
+throttle_smoother_rate: 75.0 # messages/sec
+
+# servo value (0 to 1) =  steering_angle_to_servo_gain * steering angle (radians) + steering_angle_to_servo_offset
+steering_angle_to_servo_gain: -1.2135
+steering_angle_to_servo_offset: 0.55 #0.5304
+
+# publish odom to base link tf
+vesc_to_odom/publish_tf: false
+
+# car wheelbase is about 25cm 
+wheelbase: .256
+
+vesc_driver:
+  port: /dev/sensors/vesc
+  duty_cycle_min: 0.0
+  duty_cycle_max: 0.0
+  current_min: 0.0
+  current_max: 100.0
+  brake_min: -20000.0
+  brake_max: 200000.0
+  speed_min: -23250
+  speed_max: 23250
+  position_min: 0.0
+  position_max: 0.0
+  servo_min: 0.15
+  servo_max: 0.85

vesc/package.xml

@@ -14,6 +14,10 @@
   <author email="boulet@ll.mit.edu">Michael T. Boulet</author>
   <author email="josh.whitley@autoware.org">Joshua Whitley</author>
 
+  <url type="website">http://www.ros.org/wiki/vesc</url>
+  <url type="repository">https://github.mit.edu/racecar/vesc</url>
+  <url type="bugtracker">https://github.mit.edu/racecar/vesc/issues</url>
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <exec_depend>vesc_driver</exec_depend>
@@ -23,4 +27,5 @@
   <export>
     <metapackage/>
   </export>
+
 </package>

vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h

@@ -34,6 +34,7 @@
 #include <ros/ros.h>
 #include <vesc_msgs/VescStateStamped.h>
 #include <std_msgs/Float64.h>
+#include <boost/shared_ptr.hpp>
 #include <tf/transform_broadcaster.h>
 
 namespace vesc_ackermann
@@ -58,14 +59,14 @@ class VescToOdom
 
   // odometry state
   double x_, y_, yaw_;
-  std_msgs::Float64::ConstPtr last_servo_cmd_;  ///< Last servo position commanded value
+  std_msgs::Float64::ConstPtr last_servo_cmd_;        ///< Last servo position commanded value
   vesc_msgs::VescStateStamped::ConstPtr last_state_;  ///< Last received state message
 
   // ROS services
   ros::Publisher odom_pub_;
   ros::Subscriber vesc_state_sub_;
   ros::Subscriber servo_sub_;
-  std::shared_ptr<tf::TransformBroadcaster> tf_pub_;
+  boost::shared_ptr<tf::TransformBroadcaster> tf_pub_;
 
   // ROS callbacks
   void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state);

vesc_ackermann/package.xml

@@ -17,18 +17,28 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>roslint</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>ackermann_msgs</build_depend>
+  <build_depend>vesc_msgs</build_depend>
 
-  <depend>ackermann_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>nav_msgs</depend>
-  <depend>nodelet</depend>
-  <depend>pluginlib</depend>
-  <depend>roscpp</depend>
-  <depend>std_msgs</depend>
-  <depend>tf</depend>
-  <depend>vesc_msgs</depend>
+  <exec_depend>nodelet</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>ackermann_msgs</exec_depend>
+  <exec_depend>vesc_msgs</exec_depend>
 
   <export>
     <nodelet plugin="${prefix}/vesc_ackermann_nodelet.xml"/>
   </export>
+
 </package>

vesc_ackermann/src/ackermann_to_vesc.cpp

@@ -37,28 +37,22 @@ namespace vesc_ackermann
 {
 
 template <typename T>
-inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value);
+inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value);
 
 AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh)
 {
   // get conversion parameters
-  if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_))
+  if (!getRequiredParam(nh, "speed_to_erpm_gain", speed_to_erpm_gain_))
     return;
-  if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_))
+  if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_))
     return;
-  if (!getRequiredParam(nh, "accel_to_current_gain", &accel_to_current_gain_))
+  if (!getRequiredParam(nh, "steering_angle_to_servo_gain", steering_to_servo_gain_))
     return;
-  if (!getRequiredParam(nh, "accel_to_brake_gain", &accel_to_brake_gain_))
-    return;
-  if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_))
-    return;
-  if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_))
+  if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_))
     return;
 
   // create publishers to vesc electric-RPM (speed) and servo commands
   erpm_pub_ = nh.advertise<std_msgs::Float64>("commands/motor/speed", 10);
-  current_pub_ = nh.advertise<std_msgs::Float64>("commands/motor/current", 10);
-  brake_pub_ = nh.advertise<std_msgs::Float64>("commands/motor/brake", 10);
   servo_pub_ = nh.advertise<std_msgs::Float64>("commands/servo/position", 10);
 
   // subscribe to ackermann topic
@@ -72,63 +66,22 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd)
   std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64);
   erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_;
 
-  // calc vesc current/brake (acceleration)
-  bool is_positive_accel = true;
-  std_msgs::Float64::Ptr current_msg(new std_msgs::Float64);
-  std_msgs::Float64::Ptr brake_msg(new std_msgs::Float64);
-  current_msg->data = 0;
-  brake_msg->data = 0;
-  if (cmd->drive.acceleration < 0)
-  {
-    brake_msg->data = accel_to_brake_gain_ * cmd->drive.acceleration;
-    is_positive_accel = false;
-  }
-  else
-  {
-    current_msg->data = accel_to_current_gain_ * cmd->drive.acceleration;
-  }
-
   // calc steering angle (servo)
   std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64);
   servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_;
 
   // publish
   if (ros::ok())
   {
-    // The below code attempts to stick to the previous mode until a new message forces a mode switch.
-    if (erpm_msg->data != 0 || previous_mode_speed_)
-    {
-      erpm_pub_.publish(erpm_msg);
-    }
-    else
-    {
-      if (is_positive_accel)
-      {
-        current_pub_.publish(current_msg);
-      }
-      else
-      {
-        brake_pub_.publish(brake_msg);
-      }
-    }
+    erpm_pub_.publish(erpm_msg);
     servo_pub_.publish(servo_msg);
   }
-
-  // The lines below are to determine which mode we are in so we can hold until new messages force a switch.
-  if (erpm_msg->data != 0)
-  {
-    previous_mode_speed_ = true;
-  }
-  else if (current_msg->data != 0 || brake_msg->data != 0)
-  {
-    previous_mode_speed_ = false;
-  }
 }
 
 template <typename T>
-inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value)
+inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value)
 {
-  if (nh.getParam(name, *value))
+  if (nh.getParam(name, value))
     return true;
 
   ROS_FATAL("AckermannToVesc: Parameter %s is required.", name.c_str());

vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp

@@ -23,26 +23,28 @@
 // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
 // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+#include <boost/shared_ptr.hpp>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
 #include <ros/ros.h>
 
-#include <memory>
-
 #include "vesc_ackermann/ackermann_to_vesc.h"
 
 namespace vesc_ackermann
 {
 
-class AckermannToVescNodelet: public nodelet::Nodelet
+class AckermannToVescNodelet : public nodelet::Nodelet
 {
 public:
-  AckermannToVescNodelet() {}
+  AckermannToVescNodelet()
+  {
+  }
 
 private:
   virtual void onInit(void);
 
-  std::shared_ptr<AckermannToVesc> ackermann_to_vesc_;
+  boost::shared_ptr<AckermannToVesc> ackermann_to_vesc_;
+
 };  // class AckermannToVescNodelet
 
 void AckermannToVescNodelet::onInit()

vesc_ackermann/src/vesc_to_odom.cpp

@@ -28,7 +28,6 @@
 #include "vesc_ackermann/vesc_to_odom.h"
 
 #include <cmath>
-#include <string>
 
 #include <nav_msgs/Odometry.h>
 #include <geometry_msgs/TransformStamped.h>
@@ -37,27 +36,26 @@ namespace vesc_ackermann
 {
 
 template <typename T>
-inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value);
+inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value);
 
-VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) :
-  odom_frame_("odom"), base_frame_("base_link"),
-  use_servo_cmd_(true), publish_tf_(false), x_(0.0), y_(0.0), yaw_(0.0)
+VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh)
+  : odom_frame_("odom"), base_frame_("base_link"), use_servo_cmd_(true), publish_tf_(false), x_(0.0), y_(0.0), yaw_(0.0)
 {
   // get ROS parameters
   private_nh.param("odom_frame", odom_frame_, odom_frame_);
   private_nh.param("base_frame", base_frame_, base_frame_);
   private_nh.param("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_, use_servo_cmd_);
-  if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_))
+  if (!getRequiredParam(nh, "speed_to_erpm_gain", speed_to_erpm_gain_))
     return;
-  if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_))
+  if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_))
     return;
   if (use_servo_cmd_)
   {
-    if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_))
+    if (!getRequiredParam(nh, "steering_angle_to_servo_gain", steering_to_servo_gain_))
       return;
-    if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_))
+    if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_))
       return;
-    if (!getRequiredParam(nh, "wheelbase", &wheelbase_))
+    if (!getRequiredParam(nh, "wheelbase", wheelbase_))
       return;
   }
   private_nh.param("publish_tf", publish_tf_, publish_tf_);
@@ -75,8 +73,7 @@ VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) :
   vesc_state_sub_ = nh.subscribe("sensors/core", 10, &VescToOdom::vescStateCallback, this);
   if (use_servo_cmd_)
   {
-    servo_sub_ = nh.subscribe("sensors/servo_position_command", 10,
-                              &VescToOdom::servoCmdCallback, this);
+    servo_sub_ = nh.subscribe("sensors/servo_position_command", 10, &VescToOdom::servoCmdCallback, this);
   }
 }
 
@@ -95,8 +92,7 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr&
   double current_steering_angle(0.0), current_angular_velocity(0.0);
   if (use_servo_cmd_)
   {
-    current_steering_angle =
-      (last_servo_cmd_->data - steering_to_servo_offset_) / steering_to_servo_gain_;
+    current_steering_angle = (last_servo_cmd_->data - steering_to_servo_offset_) / steering_to_servo_gain_;
     current_angular_velocity = current_speed * tan(current_steering_angle) / wheelbase_;
   }
 
@@ -136,8 +132,8 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr&
 
   // Position uncertainty
   /** @todo Think about position uncertainty, perhaps get from parameters? */
-  odom->pose.covariance[0]  = 0.2;  ///< x
-  odom->pose.covariance[7]  = 0.2;  ///< y
+  odom->pose.covariance[0] = 0.2;   ///< x
+  odom->pose.covariance[7] = 0.2;   ///< y
   odom->pose.covariance[35] = 0.4;  ///< yaw
 
   // Velocity ("in the coordinate frame given by the child_frame_id")
@@ -176,9 +172,9 @@ void VescToOdom::servoCmdCallback(const std_msgs::Float64::ConstPtr& servo)
 }
 
 template <typename T>
-inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value)
+inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value)
 {
-  if (nh.getParam(name, *value))
+  if (nh.getParam(name, value))
     return true;
 
   ROS_FATAL("VescToOdom: Parameter %s is required.", name.c_str());

vesc_ackermann/src/vesc_to_odom_nodelet.cpp

@@ -23,26 +23,28 @@
 // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
 // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+#include <boost/shared_ptr.hpp>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
 #include <ros/ros.h>
 
-#include <memory>
-
 #include "vesc_ackermann/vesc_to_odom.h"
 
 namespace vesc_ackermann
 {
 
-class VescToOdomNodelet: public nodelet::Nodelet
+class VescToOdomNodelet : public nodelet::Nodelet
 {
 public:
-  VescToOdomNodelet() {}
+  VescToOdomNodelet()
+  {
+  }
 
 private:
   virtual void onInit(void);
 
-  std::shared_ptr<VescToOdom> vesc_to_odom_;
+  boost::shared_ptr<VescToOdom> vesc_to_odom_;
+
 };  // class VescToOdomNodelet
 
 void VescToOdomNodelet::onInit()

vesc_driver/CHANGELOG.rst

@@ -9,7 +9,7 @@ Changelog for package vesc_driver
 * Exclude crc.h from roslint.
 * Replacing boost::crc with CRCPP.
 * Replacing boost::begin, boost::end, and boost::distance with Standard Library equivalents.
-* Replacing boost::bind with Standard Library equivalent.
+* Replacing boost::bind with Standards Library equivalent.
 * Replaing boost::noncopyable with C++ equivalent.
 * Replacing boost::function with Standard Library version.
 * Replacing Boost smart pointers with Standard Library equivalents.