use clipped value for servo command to estimate odometry, thanks to @…

…Winter-Guerra for calling attention to the issue
ret15108 · Sep 20, 2016 · dc4787b · dc4787b
1 parent 294d55b
commit dc4787b
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 2 deletions.
diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp
@@ -46,7 +46,8 @@ VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) :
   // subscribe to vesc state and. optionally, servo command
   vesc_state_sub_ = nh.subscribe("sensors/core", 10, &VescToOdom::vescStateCallback, this);
   if (use_servo_cmd_) {
-    servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescToOdom::servoCmdCallback, this);
+    servo_sub_ = nh.subscribe("sensors/servo_position_command", 10,
+                              &VescToOdom::servoCmdCallback, this);
   }
 }
 

diff --git a/vesc_driver/include/vesc_driver/vesc_driver.h b/vesc_driver/include/vesc_driver/vesc_driver.h
@@ -48,6 +48,7 @@ private:
 
   // ROS services
   ros::Publisher state_pub_;
+  ros::Publisher servo_sensor_pub_;
   ros::Subscriber duty_cycle_sub_;
   ros::Subscriber current_sub_;
   ros::Subscriber brake_sub_;

diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp
@@ -43,6 +43,10 @@ VescDriver::VescDriver(ros::NodeHandle nh,
   // create vesc state (telemetry) publisher
   state_pub_ = nh.advertise<vesc_msgs::VescStateStamped>("sensors/core", 10);
 
+  // since vesc state does not include the servo position, publish the commanded
+  // servo position as a "sensor"
+  servo_sensor_pub_ = nh.advertise<std_msgs::Float64>("sensors/servo_position_command", 10);
+
   // subscribe to motor and servo command topics
   duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10,
                                  &VescDriver::dutyCycleCallback, this);
@@ -209,7 +213,12 @@ void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position)
 void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo)
 {
   if (driver_mode_ = MODE_OPERATING) {
-    vesc_.setServo(servo_limit_.clip(servo->data));
+    double servo_clipped(servo_limit_.clip(servo->data));
+    vesc_.setServo(servo_clipped);
+    // publish clipped servo value as a "sensor"
+    std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64);
+    servo_sensor_msg->data = servo_clipped;
+    servo_sensor_pub_.publish(servo_sensor_msg);
   }
 }