From dc4787b56f5065e0a0a74d0f220dcda5b3501129 Mon Sep 17 00:00:00 2001 From: Michael Boulet Date: Tue, 20 Sep 2016 13:08:31 -0400 Subject: [PATCH] use clipped value for servo command to estimate odometry, thanks to @Winter-Guerra for calling attention to the issue --- vesc_ackermann/src/vesc_to_odom.cpp | 3 ++- vesc_driver/include/vesc_driver/vesc_driver.h | 1 + vesc_driver/src/vesc_driver.cpp | 11 ++++++++++- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index 95c5abe..eba416a 100644 --- 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 index 835d685..005c0be 100644 --- 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 index a9111b5..e496f6b 100644 --- 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("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("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); } }