Added axis_min_velocity configuration parameter to make it easier to …

…work with motors which are unstable at low velocities

README.md

@@ -38,6 +38,7 @@ Driver is configured with the following configuration paramters:
 | ```can_tx_ropic```      | String   | Topic name through which driver sends CAN messages to socketcan_bridge             |
 | ```update_rate```       | Double   | Update rate in Hz of how frequently driver gets data from ODrive controllers       |
 | ```engage_on_startup``` | Boolean  | If driver should engage all motors on startup (not implemented yet)                |
+| ```axis_min_velocity``` | Double   | Minimum axis velocity, if target velocity is less then this value then it is set to this value (if set to 1.0 target velocity of 0.5 will become 1.0, target velocity of -0.5 will become -1.0) |
 | ```axis_names```        | String[] | Array of names of axises which are then used to name topics corresponding to axises|
 | ```axis_can_ids```      | Int[]    | Array of axis CAN ids as they are configured on ODrive controllers for every axis  |
 | ```axis_directions```   | String[] | Axis direction either ```forward``` or ```backward```, ```backward``` reverses axis control, angle and current velocity |
@@ -61,6 +62,7 @@ When driver is started it creates the following topics for every axis configured
 |```<axis name>/angle```            | ```std_msgs/Float64``` | Outbound  | Angle of the axis in rad               |
 |```<axis name>/current```          | ```std_msgs/Float64``` | Outbound  | Axis current consumption               |
 |```<axis name>/current_velocity``` | ```std_msgs/Float64``` | Outbound  | Current axis angular velocity in rad/s |
+|```<axis name>/output_velocity```  | ```std_msgs/Float64``` | Outbound  | Velocity which is sent to ODrive (will differ from target_velocity if axis_min_velocity is set) |
 |```<axis name>/target_velocity```  | ```std_msgs/Float64``` | Inbound   | Target axis angular velocity in rad/s  |
 |```<axis name>/voltage```          | ```std_msgs/Float64``` | Outbound  | Axis voltage of power supply in V      |
 

config/odrive.yaml

@@ -8,6 +8,9 @@ can_tx_ropic: "/can/sent_messages"
 update_rate: 10
 # engage axises on startup
 engage_on_startup: true
+# minimum axis velocity - e.g if set to 1.0 target velocity will be set to this value
+# for both positive and negative velocities (0.5 will become 1.0, -0.5 will become -1.0)
+axis_min_velocity: 1.0
 # array of strings representing names of axises
 axis_names: ["front_left_wheel", "front_right_wheel", "middle_left_wheel", "middle_right_wheel", "rear_left_wheel", "rear_right_wheel"]
 # array of ints representing can ids of axises

src/odrive_axis.cpp

@@ -13,11 +13,13 @@ namespace odrive {
         node->param<std::string>("can_tx_topic", can_tx_topic_, "/can/sent_messages");
         node->param<double>("update_rate", update_rate_, DEFAULT_UPDATE_RATE);
         node->param<bool>("engage_on_startup", engage_on_startup_, false);
+        node->param<double>("axis_min_velocity", axis_min_velocity_, 0);
         received_messages_sub_ = node->subscribe<can_msgs::Frame>(can_rx_topic_, 1,
             std::bind(&ODriveAxis::canReceivedMessagesCallback, this, std::placeholders::_1));
         sent_messages_pub_ = node->advertise<can_msgs::Frame>(can_tx_topic_, 1);
         target_velocity_sub_ = node->subscribe<std_msgs::Float64>("/" + axis_name_ + "/target_velocity",
             1, std::bind(&ODriveAxis::velocityReceivedMessagesCallback, this, std::placeholders::_1));
+        output_velocity_pub_ = node->advertise<std_msgs::Float64>("/" + axis_name + "/output_velocity", 1);
         axis_angle_pub_ = node->advertise<std_msgs::Float64>("/" + axis_name + "/angle", 1);
         axis_velocity_pub_ = node->advertise<std_msgs::Float64>("/" + axis_name + "/current_velocity", 1);
         axis_voltage_pub_ = node->advertise<std_msgs::Float64>("/" + axis_name + "/voltage", 1);
@@ -50,7 +52,7 @@ namespace odrive {
         uint8_t *ptrPositionEstimate = (uint8_t *)&positionEstimate;
         uint8_t *ptrAxisVoltage = (uint8_t *)&axisVoltage;
         uint8_t *ptrAxisCurrent = (uint8_t *)&axisCurrent;
-        if (NODE_ID == axis_can_id_) {
+        if (NODE_ID == (uint32_t)axis_can_id_) {
             uint32_t CMD_ID = (ID & 0x01F);
             switch (CMD_ID) {
             case ODriveCommandId::HEARTBEAT_MESSAGE:
@@ -98,12 +100,29 @@ namespace odrive {
     }
 
     void ODriveAxis::velocityReceivedMessagesCallback(const std_msgs::Float64::ConstPtr& msg) {
-        ROS_DEBUG("Received velocity message");
-        double targetVelocity = msg->data / (2.0 * M_PI) * direction_;
-        setInputVelocity(targetVelocity);
+        std_msgs::Float64 velocity_msg;
+        double targetVelocity = msg->data;
+        // Check that target velocity is not less then axis minimum velocity in both directions
+        if (targetVelocity != 0 && axis_min_velocity_ != 0) {
+            if (targetVelocity > 0) {
+                if (targetVelocity < axis_min_velocity_) {
+                    targetVelocity = axis_min_velocity_;
+                }
+            } else {
+                if (targetVelocity > (axis_min_velocity_ * -1)) {
+                    targetVelocity = axis_min_velocity_ * -1;
+                }
+            }
+        }
+        // Publish velocity to output_velocity
+        velocity_msg.data = (double)targetVelocity;
+        output_velocity_pub_.publish(velocity_msg);
+        double odriveTargetVelocity = targetVelocity / (2.0 * M_PI) * direction_;
+        setInputVelocity(odriveTargetVelocity);
     }
 
     void ODriveAxis::updateTimerCallback(const ros::TimerEvent& event) {
+        (void)event;
         requestEncoderEstimate();
         requestBusVoltageAndCurrent();
     }
@@ -173,7 +192,8 @@ namespace odrive {
 
 
     uint32_t ODriveAxis::createCanId(int axis_can_id, int command) {
-        uint32_t can_id = (axis_can_id_ << 5) | command;
+        uint32_t can_id;
+        can_id = (axis_can_id << 5) | command;
         return can_id;
     }
 

src/odrive_axis.hpp

@@ -68,18 +68,20 @@ namespace odrive {
             ros::Subscriber received_messages_sub_;
             ros::Publisher sent_messages_pub_;
             ros::Subscriber target_velocity_sub_;
+            ros::Publisher output_velocity_pub_;
             ros::Publisher axis_angle_pub_;
-            ros::Publisher axis_velocity_pub_;            
-            ros::Publisher axis_voltage_pub_;            
-            ros::Publisher axis_current_pub_;            
-            ros::Timer axis_update_timer_; 
+            ros::Publisher axis_velocity_pub_;
+            ros::Publisher axis_voltage_pub_;
+            ros::Publisher axis_current_pub_;
+            ros::Timer axis_update_timer_;
             std::string axis_name_;
             std::string can_rx_topic_;
             std::string can_tx_topic_;
             int axis_can_id_;
             int direction_;
             bool engage_on_startup_;
             double update_rate_;
+            double axis_min_velocity_;
             double axis_angle_;
             double axis_velocity_;
             double axis_voltage_;

src/odrive_node.cpp

@@ -68,7 +68,7 @@ int main(int argc, char** argv) {
         ROS_ERROR("axis CAN ids must be distinct");
         return -1;
     }
-    for (int i = 0; i < axis_names_list.size(); i++) {
+    for (int i = 0; i < (int)axis_names_list.size(); i++) {
         ROS_INFO("Adding axis %s with CAN id %d and direction %s", axis_names_list[i].c_str(), 
             axis_can_ids_list[i], axis_directions_list[i].c_str());
         if (axis_names_list[i].length() == 0) {