Rotor position publisher

vesc_driver/include/vesc_driver/vesc_driver.h

@@ -55,6 +55,7 @@ class VescDriver
   ros::Subscriber speed_sub_;
   ros::Subscriber position_sub_;
   ros::Subscriber servo_sub_;
+  ros::Publisher rotor_position_pub_;
   ros::Timer timer_;
 
   // driver modes (possible states)

vesc_driver/include/vesc_driver/vesc_interface.h

@@ -14,6 +14,7 @@
 #include <boost/shared_ptr.hpp>
 
 #include "vesc_driver/vesc_packet.h"
+#include "vesc_driver/datatypes.h"
 
 namespace vesc_driver
 {
@@ -91,6 +92,7 @@ class VescInterface : private boost::noncopyable
   void setSpeed(double speed);
   void setPosition(double position);
   void setServo(double servo);
+  void setDetect(disp_pos_mode mode);
 
 private:
   // Pimpl - hide serial port members from class users

vesc_driver/include/vesc_driver/vesc_packet.h

@@ -75,6 +75,18 @@ class VescPacket : public VescFrame
 typedef boost::shared_ptr<VescPacket> VescPacketPtr;
 typedef boost::shared_ptr<VescPacket const> VescPacketConstPtr;
 
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketRotorPosition : public VescPacket
+{
+public:
+  VescPacketRotorPosition(boost::shared_ptr<VescFrame> raw);
+
+  float position() const;
+
+};
+
+
 /*------------------------------------------------------------------------------------------------*/
 
 class VescPacketFWVersion : public VescPacket
@@ -189,6 +201,14 @@ class VescPacketSetServoPos : public VescPacket
   //  double servo_pos() const;
 };
 
+/*------------------------------------------------------------------------------------------------*/
+
+class VescPacketSetDetect : public VescPacket
+{
+public:
+  VescPacketSetDetect(uint8_t mode);
+};
+
 } // namespace vesc_driver
 
 #endif // VESC_DRIVER_VESC_PACKET_H_

vesc_driver/launch/vesc_driver_node.launch

@@ -11,10 +11,15 @@
   <!-- VESC driver parameters -->
   <arg name="port" default="/dev/ttyUSB0" />
 
+  <!-- Parameter for enabling rotor position publishing
+       Possible values are "inductance", "observer", "encoder", "pid_pos", "pid_pos_error", "encoder_observer_error" -->
+  <arg name="rotor_position_source" default="none" />
+
   <!-- 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="rotor_position_source" value="$(arg rotor_position_source)" />
   </node>
 
 </launch>

vesc_driver/launch/vesc_driver_nodelet.launch

@@ -14,7 +14,11 @@
 
   <!-- VESC driver parameters -->
   <arg name="port" default="/dev/ttyUSB0" />
-
+
+  <!-- Parameter for enabling rotor position publishing
+       Possible values are "inductance", "observer", "encoder", "pid_pos", "pid_pos_error", "encoder_observer_error" -->
+  <arg name="rotor_position_source" default="none" />
+
   <!-- Also globally disable bond heartbeat timeout in debug mode, so everything
        doesn't die when you hit a break point -->
   <param if="$(arg debug)" name="/bond_disable_heartbeat_timeout" value="true" />
@@ -29,6 +33,7 @@
   <node pkg="nodelet" type="nodelet" name="$(arg node_name)"
         args="load vesc_driver::VescDriverNodelet $(arg manager)" >
     <param name="port" value="$(arg port)" />
-  </node>    
+    <param name="rotor_position_source" value="$(arg rotor_position_source)" />
+  </node>
 
 </launch>

vesc_driver/src/vesc_driver.cpp

@@ -1,13 +1,14 @@
 // -*- mode:c++; fill-column: 100; -*-
-
 #include "vesc_driver/vesc_driver.h"
+#include "vesc_driver/datatypes.h"
 
 #include <cassert>
 #include <cmath>
 #include <sstream>
 
 #include <boost/bind.hpp>
 #include <vesc_msgs/VescStateStamped.h>
+#include <std_msgs/Float32.h>
 
 namespace vesc_driver
 {
@@ -58,6 +59,36 @@ VescDriver::VescDriver(ros::NodeHandle nh,
 
   // create a 50Hz timer, used for state machine & polling VESC telemetry
   timer_ = nh.createTimer(ros::Duration(1.0/50.0), &VescDriver::timerCallback, this);
+
+  // decide whether or not we want to publish rotor positions
+  // this is enabled with by sending a "set detect" packet with a mode
+  // "encoder" is probably the most useful
+  std::string rotor_position_source;
+  if (private_nh.getParam("rotor_position_source", rotor_position_source)) {
+    disp_pos_mode mode = DISP_POS_MODE_NONE;
+    if (rotor_position_source.compare("inductance") == 0) {
+      mode = DISP_POS_MODE_INDUCTANCE;
+    }  else if (rotor_position_source.compare("observer") == 0) {
+      mode = DISP_POS_MODE_OBSERVER;
+    } else if (rotor_position_source.compare("encoder") == 0) {
+      mode = DISP_POS_MODE_ENCODER;
+    } else if (rotor_position_source.compare("pid_pos") == 0) {
+      mode = DISP_POS_MODE_PID_POS;
+    } else if (rotor_position_source.compare("pid_pos_error") == 0) {
+      mode = DISP_POS_MODE_PID_POS_ERROR;
+    } else if (rotor_position_source.compare("encoder_observer_error") == 0) {
+      mode = DISP_POS_MODE_ENCODER_OBSERVER_ERROR;
+    } else if (rotor_position_source.compare("none") != 0) {
+      ROS_WARN("Invalid display mode parameter, defaulting to none");
+    }
+
+    if (mode != DISP_POS_MODE_NONE) {
+      // create rotor position publisher
+      rotor_position_pub_ = nh.advertise<std_msgs::Float32>("sensors/rotor_position", 10);
+      ROS_INFO("Enabling rotor position publisher from source: %s", rotor_position_source.c_str());
+    }
+    vesc_.setDetect(mode);
+  }
 }
 
   /* TODO or TO-THINKABOUT LIST
@@ -109,7 +140,7 @@ void VescDriver::timerCallback(const ros::TimerEvent& event)
 
 void VescDriver::vescPacketCallback(const boost::shared_ptr<VescPacket const>& packet)
 {
-  if (packet->name() == "Values") {
+  if (packet->name() == "Values" && state_pub_) {
     boost::shared_ptr<VescPacketValues const> values =
       boost::dynamic_pointer_cast<VescPacketValues const>(packet);
 
@@ -138,6 +169,16 @@ void VescDriver::vescPacketCallback(const boost::shared_ptr<VescPacket const>& p
     fw_version_major_ = fw_version->fwMajor();
     fw_version_minor_ = fw_version->fwMinor();
   }
+  else if (packet->name() == "RotorPosition" && rotor_position_pub_) {
+    //pubish the value of the rotor position
+    boost::shared_ptr<VescPacketRotorPosition const> position =
+      boost::dynamic_pointer_cast<VescPacketRotorPosition const>(packet);
+
+    std_msgs::Float32::Ptr rotor_position_msg(new std_msgs::Float32);
+    // we get degrees from the VESC, but radians are the standard in ROS
+    rotor_position_msg->data = position->position() * M_PI / 180.0;
+    rotor_position_pub_.publish(rotor_position_msg);
+  }
 }
 
 void VescDriver::vescErrorCallback(const std::string& error)

vesc_driver/src/vesc_interface.cpp

@@ -13,6 +13,7 @@
 #include <boost/crc.hpp>
 
 #include "vesc_driver/vesc_packet_factory.h"
+#include "vesc_driver/datatypes.h"
 
 namespace vesc_driver
 {
@@ -241,4 +242,9 @@ void VescInterface::setServo(double servo)
   send(VescPacketSetServoPos(servo));
 }
 
+void VescInterface::setDetect(disp_pos_mode mode)
+{
+  send(VescPacketSetDetect(mode));
+}
+
 } // namespace vesc_driver

vesc_driver/src/vesc_packet.cpp

@@ -69,6 +69,26 @@ VescPacket::VescPacket(const std::string& name, boost::shared_ptr<VescFrame> raw
 
 /*------------------------------------------------------------------------------------------------*/
 
+VescPacketRotorPosition::VescPacketRotorPosition(boost::shared_ptr<VescFrame> raw) :
+  VescPacket("RotorPosition", raw)
+{
+}
+
+float VescPacketRotorPosition::position() const
+{
+  int32_t value = 0;
+  value |= *(payload_.first + 1) << 24;
+  value |= *(payload_.first + 2) << 16;
+  value |= *(payload_.first + 3) << 8;
+  value |= *(payload_.first + 4);
+
+  return value / 100000.0;
+}
+
+REGISTER_PACKET_TYPE(COMM_ROTOR_POSITION, VescPacketRotorPosition)
+
+/*------------------------------------------------------------------------------------------------*/
+
 VescPacketFWVersion::VescPacketFWVersion(boost::shared_ptr<VescFrame> raw) :
   VescPacket("FWVersion", raw)
 {
@@ -365,4 +385,18 @@ VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) :
   *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
 }
 
+/*------------------------------------------------------------------------------------------------*/
+
+VescPacketSetDetect::VescPacketSetDetect(uint8_t mode) :
+  VescPacket("SetDetect", 3, COMM_SET_DETECT)
+{
+  *(payload_.first + 1) = mode;
+
+  VescFrame::CRC crc_calc;
+  crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_));
+  uint16_t crc = crc_calc.checksum();
+  *(frame_->end() - 3) = static_cast<uint8_t>(crc >> 8);
+  *(frame_->end() - 2) = static_cast<uint8_t>(crc & 0xFF);
+}
+
 } // namespace vesc_driver