From c4ffaa45cbdf5d437165e1d191d58e87721a572a Mon Sep 17 00:00:00 2001 From: Michael Boulet Date: Thu, 14 Jan 2016 23:43:30 -0500 Subject: [PATCH] add vesc_ackermann package for translating between vesc and ackermann and odometry messages --- vesc_ackermann/CMakeLists.txt | 74 +++++++++ .../vesc_ackermann/ackermann_to_vesc.h | 37 +++++ .../include/vesc_ackermann/vesc_to_odom.h | 47 ++++++ .../launch/ackermann_to_vesc_node.launch | 14 ++ .../launch/ackermann_to_vesc_nodelet.launch | 29 ++++ .../launch/vesc_to_odom_node.launch | 14 ++ .../launch/vesc_to_odom_nodelet.launch | 29 ++++ vesc_ackermann/package.xml | 39 +++++ vesc_ackermann/src/ackermann_to_vesc.cpp | 64 ++++++++ vesc_ackermann/src/ackermann_to_vesc_node.cpp | 16 ++ .../src/ackermann_to_vesc_nodelet.cpp | 33 ++++ vesc_ackermann/src/vesc_to_odom.cpp | 126 +++++++++++++++ vesc_ackermann/src/vesc_to_odom_node.cpp | 16 ++ vesc_ackermann/src/vesc_to_odom_nodelet.cpp | 33 ++++ vesc_ackermann/vesc_ackermann_nodelet.xml | 8 + vesc_driver/include/vesc_driver/vesc_driver.h | 19 +++ vesc_driver/launch/vesc_driver_nodelet.launch | 2 +- vesc_driver/src/vesc_driver.cpp | 152 +++++++++++++----- vesc_msgs/CMakeLists.txt | 1 + vesc_msgs/msg/VescState.msg | 1 - vesc_msgs/msg/VescStateStamped.msg | 4 + 21 files changed, 715 insertions(+), 43 deletions(-) create mode 100644 vesc_ackermann/CMakeLists.txt create mode 100644 vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h create mode 100644 vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h create mode 100644 vesc_ackermann/launch/ackermann_to_vesc_node.launch create mode 100644 vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch create mode 100644 vesc_ackermann/launch/vesc_to_odom_node.launch create mode 100644 vesc_ackermann/launch/vesc_to_odom_nodelet.launch create mode 100644 vesc_ackermann/package.xml create mode 100644 vesc_ackermann/src/ackermann_to_vesc.cpp create mode 100644 vesc_ackermann/src/ackermann_to_vesc_node.cpp create mode 100644 vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp create mode 100644 vesc_ackermann/src/vesc_to_odom.cpp create mode 100644 vesc_ackermann/src/vesc_to_odom_node.cpp create mode 100644 vesc_ackermann/src/vesc_to_odom_nodelet.cpp create mode 100644 vesc_ackermann/vesc_ackermann_nodelet.xml create mode 100644 vesc_msgs/msg/VescStateStamped.msg diff --git a/vesc_ackermann/CMakeLists.txt b/vesc_ackermann/CMakeLists.txt new file mode 100644 index 0000000..9e762e8 --- /dev/null +++ b/vesc_ackermann/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 2.8.3) +project(vesc_ackermann) + +find_package(catkin REQUIRED COMPONENTS + nodelet + pluginlib + roscpp + nav_msgs + std_msgs + ackermann_msgs + vesc_msgs +) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS nodelet pluginlib roscpp nav_msgs std_msgs ackermann_msgs vesc_msgs +) + +########### +## Build ## +########### + +include_directories( + include + ${Boost_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} +) + +# node executable +add_executable(vesc_to_odom_node src/vesc_to_odom_node.cpp + src/vesc_to_odom.cpp) +target_link_libraries(vesc_to_odom_node + ${catkin_LIBRARIES} +) + +add_executable(ackermann_to_vesc_node src/ackermann_to_vesc_node.cpp + src/ackermann_to_vesc.cpp) +target_link_libraries(ackermann_to_vesc_node + ${catkin_LIBRARIES} +) + +# nodelet library +add_library(vesc_ackermann_nodelet src/ackermann_to_vesc_nodelet.cpp + src/ackermann_to_vesc.cpp + src/vesc_to_odom_nodelet.cpp + src/vesc_to_odom.cpp) +target_link_libraries(vesc_ackermann_nodelet + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +install(TARGETS vesc_to_odom_node ackermann_to_vesc_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS vesc_ackermann_nodelet + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(FILES vesc_ackermann_nodelet.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) + +############# +## Testing ## +############# + +# TODO \ No newline at end of file diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h new file mode 100644 index 0000000..3edb06e --- /dev/null +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h @@ -0,0 +1,37 @@ +// -*- mode:c++; fill-column: 100; -*- + +#ifndef VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ +#define VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ + +#include +#include + +namespace vesc_ackermann +{ + +class AckermannToVesc +{ +public: + + AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh); + +private: + // ROS parameters + // conversion gain and offset + double speed_to_erpm_gain_, speed_to_erpm_offset_; + double steering_to_servo_gain_, steering_to_servo_offset_; + + /** @todo consider also providing an interpolated look-up table conversion */ + + // ROS services + ros::Publisher erpm_pub_; + ros::Publisher servo_pub_; + ros::Subscriber ackermann_sub_; + + // ROS callbacks + void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& cmd); +}; + +} // namespace vesc_ackermann + +#endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h new file mode 100644 index 0000000..66a23ec --- /dev/null +++ b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h @@ -0,0 +1,47 @@ +// -*- mode:c++; fill-column: 100; -*- + +#ifndef VESC_ACKERMANN_VESC_TO_ODOM_H_ +#define VESC_ACKERMANN_VESC_TO_ODOM_H_ + +#include +#include +#include + +namespace vesc_ackermann +{ + +class VescToOdom +{ +public: + + VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh); + +private: + // ROS parameters + std::string odom_frame_; + std::string base_frame_; + /** State message does not report servo position, so use the command instead */ + bool use_servo_cmd_; + // conversion gain and offset + double speed_to_erpm_gain_, speed_to_erpm_offset_; + double steering_to_servo_gain_, steering_to_servo_offset_; + double wheelbase_; + + // odometry state + double x_, y_, yaw_; + 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_; + + // ROS callbacks + void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state); + void servoCmdCallback(const std_msgs::Float64::ConstPtr& servo); +}; + +} // namespace vesc_ackermann + +#endif // VESC_ACKERMANN_VESC_TO_ODOM_H_ diff --git a/vesc_ackermann/launch/ackermann_to_vesc_node.launch b/vesc_ackermann/launch/ackermann_to_vesc_node.launch new file mode 100644 index 0000000..d0b2ea9 --- /dev/null +++ b/vesc_ackermann/launch/ackermann_to_vesc_node.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch b/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch new file mode 100644 index 0000000..5060fe3 --- /dev/null +++ b/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vesc_ackermann/launch/vesc_to_odom_node.launch b/vesc_ackermann/launch/vesc_to_odom_node.launch new file mode 100644 index 0000000..a2cf374 --- /dev/null +++ b/vesc_ackermann/launch/vesc_to_odom_node.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/vesc_ackermann/launch/vesc_to_odom_nodelet.launch b/vesc_ackermann/launch/vesc_to_odom_nodelet.launch new file mode 100644 index 0000000..19a3aab --- /dev/null +++ b/vesc_ackermann/launch/vesc_to_odom_nodelet.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml new file mode 100644 index 0000000..0c9ea6c --- /dev/null +++ b/vesc_ackermann/package.xml @@ -0,0 +1,39 @@ + + + vesc_ackermann + 0.0.1 + + Translate between VESC messages and ROS ackermann and odometry messages. + + + Michael T. Boulet + Michael T. Boulet + BSD + + http://www.ros.org/wiki/vesc_ackermann + https://github.mit.edu/racecar/racecar-iap + https://github.mit.edu/racecar/racecar-iap/issues + + catkin + + nodelet + pluginlib + roscpp + nav_msgs + std_msgs + ackermann_msgs + vesc_msgs + + nodelet + pluginlib + roscpp + nav_msgs + std_msgs + ackermann_msgs + vesc_msgs + + + + + + diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp new file mode 100644 index 0000000..0a00aaa --- /dev/null +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -0,0 +1,64 @@ +// -*- mode:c++; fill-column: 100; -*- + +#include "vesc_ackermann/ackermann_to_vesc.h" + +#include +#include + +#include + +namespace vesc_ackermann +{ + +template +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_)) + return; + if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_)) + 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_)) + return; + + // create publishers to vesc electric-RPM (speed) and servo commands + erpm_pub_ = nh.advertise("commands/motor/speed", 10); + servo_pub_ = nh.advertise("commands/servo/position", 10); + + // subscribe to ackermann topic + ackermann_sub_ = nh.subscribe("ackermann_cmd", 10, &AckermannToVesc::ackermannCmdCallback, this); +} + +typedef ackermann_msgs::AckermannDriveStamped::ConstPtr AckermannMsgPtr; +void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) +{ + // calc vesc electric RPM (speed) + 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 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()) { + erpm_pub_.publish(erpm_msg); + servo_pub_.publish(servo_msg); + } +} + +template +inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value) +{ + if (nh.getParam(name, value)) + return true; + + ROS_FATAL("AckermannToVesc: Parameter %s is required.", name.c_str()); + return false; +} + +} // namespace vesc_ackermann diff --git a/vesc_ackermann/src/ackermann_to_vesc_node.cpp b/vesc_ackermann/src/ackermann_to_vesc_node.cpp new file mode 100644 index 0000000..cfe2268 --- /dev/null +++ b/vesc_ackermann/src/ackermann_to_vesc_node.cpp @@ -0,0 +1,16 @@ +#include + +#include "vesc_ackermann/ackermann_to_vesc.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "ackermann_to_vesc_node"); + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + + vesc_ackermann::AckermannToVesc ackermann_to_vesc(nh, private_nh); + + ros::spin(); + + return 0; +} diff --git a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp new file mode 100644 index 0000000..129df4f --- /dev/null +++ b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp @@ -0,0 +1,33 @@ +#include +#include +#include +#include + +#include "vesc_ackermann/ackermann_to_vesc.h" + +namespace vesc_ackermann +{ + +class AckermannToVescNodelet: public nodelet::Nodelet +{ +public: + + AckermannToVescNodelet() {} + +private: + + virtual void onInit(void); + + boost::shared_ptr ackermann_to_vesc_; + +}; // class AckermannToVescNodelet + +void AckermannToVescNodelet::onInit() +{ + NODELET_DEBUG("Initializing ackermann to VESC nodelet"); + ackermann_to_vesc_.reset(new AckermannToVesc(getNodeHandle(), getPrivateNodeHandle())); +} + +} // namespace vesc_ackermann + +PLUGINLIB_EXPORT_CLASS(vesc_ackermann::AckermannToVescNodelet, nodelet::Nodelet); diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp new file mode 100644 index 0000000..00d1430 --- /dev/null +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -0,0 +1,126 @@ +// -*- mode:c++; fill-column: 100; -*- + +#include "vesc_ackermann/vesc_to_odom.h" + +#include + +#include "nav_msgs/Odometry.h" + +namespace vesc_ackermann +{ + +template +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), 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_)) + return; + 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_)) + return; + if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_)) + return; + if (!getRequiredParam(nh, "wheelbase", wheelbase_)) + return; + } + + // create odom publisher + odom_pub_ = nh.advertise("odom", 10); + + // 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); + } +} + +void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state) +{ + // check that we have a last servo command if we are depending on it for angular velocity + if (use_servo_cmd_ && !last_servo_cmd_) + return; + + // convert to engineering units + double current_speed = ( state->state.speed - speed_to_erpm_offset_ ) / speed_to_erpm_gain_; + 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_angular_velocity = current_speed * tan(current_steering_angle) / wheelbase_; + } + + // use current state as last state if this is our first time here + if (!last_state_) + last_state_ = state; + + // calc elapsed time + ros::Duration dt = state->header.stamp - last_state_->header.stamp; + + /** @todo could probably do better propigating odometry, e.g. trapezoidal integration */ + + // propigate odometry + double x_dot = current_speed * cos(yaw_); + double y_dot = current_speed * sin(yaw_); + x_ += x_dot * dt.toSec(); + y_ += y_dot * dt.toSec(); + if (use_servo_cmd_) + yaw_ += current_angular_velocity * dt.toSec(); + + // publish odometry message + nav_msgs::Odometry::Ptr odom(new nav_msgs::Odometry); + odom->header.frame_id = odom_frame_; + odom->header.stamp = state->header.stamp; + odom->child_frame_id = base_frame_; + + // Position + odom->pose.pose.position.x = x_; + odom->pose.pose.position.x = y_; + odom->pose.pose.orientation.x = 0.0; + odom->pose.pose.orientation.y = 0.0; + odom->pose.pose.orientation.z = sin(yaw_/2); + odom->pose.pose.orientation.w = cos(yaw_/2); + + // Position uncertainty + /** @todo Think about position uncertainty, perhaps get from parameters? */ + odom->pose.covariance[0] = 0.1; ///< x + odom->pose.covariance[7] = 0.1; ///< y + odom->pose.covariance[35] = 0.2; ///< yaw + + // Velocity + odom->twist.twist.linear.x = x_dot; + odom->twist.twist.linear.y = y_dot; + odom->twist.twist.angular.z = current_angular_velocity; + + // Velocity uncertainty + /** @todo Think about velocity uncertainty */ + + if (ros::ok()) { + odom_pub_.publish(odom); + } +} + +void VescToOdom::servoCmdCallback(const std_msgs::Float64::ConstPtr& servo) +{ + last_servo_cmd_ = servo; +} + +template +inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value) +{ + if (nh.getParam(name, value)) + return true; + + ROS_FATAL("VescToOdom: Parameter %s is required.", name.c_str()); + return false; +} + +} // namespace vesc_ackermann diff --git a/vesc_ackermann/src/vesc_to_odom_node.cpp b/vesc_ackermann/src/vesc_to_odom_node.cpp new file mode 100644 index 0000000..9ce992f --- /dev/null +++ b/vesc_ackermann/src/vesc_to_odom_node.cpp @@ -0,0 +1,16 @@ +#include + +#include "vesc_ackermann/vesc_to_odom.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "vesc_to_odom_node"); + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + + vesc_ackermann::VescToOdom vesc_to_odom(nh, private_nh); + + ros::spin(); + + return 0; +} diff --git a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp new file mode 100644 index 0000000..3fc37e4 --- /dev/null +++ b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp @@ -0,0 +1,33 @@ +#include +#include +#include +#include + +#include "vesc_ackermann/vesc_to_odom.h" + +namespace vesc_ackermann +{ + +class VescToOdomNodelet: public nodelet::Nodelet +{ +public: + + VescToOdomNodelet() {} + +private: + + virtual void onInit(void); + + boost::shared_ptr vesc_to_odom_; + +}; // class VescToOdomNodelet + +void VescToOdomNodelet::onInit() +{ + NODELET_DEBUG("Initializing RACECAR VESC odometry estimator nodelet"); + vesc_to_odom_.reset(new VescToOdom(getNodeHandle(), getPrivateNodeHandle())); +} + +} // namespace vesc_ackermann + +PLUGINLIB_EXPORT_CLASS(vesc_ackermann::VescToOdomNodelet, nodelet::Nodelet); diff --git a/vesc_ackermann/vesc_ackermann_nodelet.xml b/vesc_ackermann/vesc_ackermann_nodelet.xml new file mode 100644 index 0000000..b12358a --- /dev/null +++ b/vesc_ackermann/vesc_ackermann_nodelet.xml @@ -0,0 +1,8 @@ + + + Nodelet for translating ackermann messages to VESC motor controller commands. + + + Nodelet for translating VESC motor controller state to odometry messages. + + diff --git a/vesc_driver/include/vesc_driver/vesc_driver.h b/vesc_driver/include/vesc_driver/vesc_driver.h index 2b7f6dc..835d685 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.h +++ b/vesc_driver/include/vesc_driver/vesc_driver.h @@ -7,6 +7,7 @@ #include #include +#include #include "vesc_driver/vesc_interface.h" #include "vesc_driver/vesc_packet.h" @@ -27,6 +28,24 @@ private: void vescPacketCallback(const boost::shared_ptr& packet); void vescErrorCallback(const std::string& error); + // limits on VESC commands + struct CommandLimit + { + CommandLimit(const ros::NodeHandle& nh, const std::string& str, + const boost::optional& min_lower = boost::optional(), + const boost::optional& max_upper = boost::optional()); + double clip(double value); + std::string name; + boost::optional lower; + boost::optional upper; + }; + CommandLimit duty_cycle_limit_; + CommandLimit current_limit_; + CommandLimit brake_limit_; + CommandLimit speed_limit_; + CommandLimit position_limit_; + CommandLimit servo_limit_; + // ROS services ros::Publisher state_pub_; ros::Subscriber duty_cycle_sub_; diff --git a/vesc_driver/launch/vesc_driver_nodelet.launch b/vesc_driver/launch/vesc_driver_nodelet.launch index bf7bae4..3d45937 100644 --- a/vesc_driver/launch/vesc_driver_nodelet.launch +++ b/vesc_driver/launch/vesc_driver_nodelet.launch @@ -2,7 +2,7 @@ - + diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index a390c9d..2e400c8 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -4,9 +4,10 @@ #include #include +#include #include -#include +#include namespace vesc_driver { @@ -16,6 +17,9 @@ VescDriver::VescDriver(ros::NodeHandle nh, vesc_(std::string(), boost::bind(&VescDriver::vescPacketCallback, this, _1), boost::bind(&VescDriver::vescErrorCallback, this, _1)), + duty_cycle_limit_(private_nh, "duty_cycle", 0.0, 1.0), current_limit_(private_nh, "current"), + brake_limit_(private_nh, "brake"), speed_limit_(private_nh, "speed"), + position_limit_(private_nh, "position"), servo_limit_(private_nh, "servo", 0.0, 1.0), driver_mode_(MODE_INITIALIZING), fw_version_major_(-1), fw_version_minor_(-1) { // get vesc serial port address @@ -37,19 +41,16 @@ VescDriver::VescDriver(ros::NodeHandle nh, } // create vesc state (telemetry) publisher - state_pub_ = private_nh.advertise("sensors/core", 10); + state_pub_ = nh.advertise("sensors/core", 10); // subscribe to motor and servo command topics - duty_cycle_sub_ = private_nh.subscribe("commands/motor/duty_cycle", - 10, &VescDriver::dutyCycleCallback, this); - current_sub_ = private_nh.subscribe("commands/motor/current", 10, - &VescDriver::currentCallback, this); - brake_sub_ = private_nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); - speed_sub_ = private_nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); - position_sub_ = private_nh.subscribe("commands/motor/position", 10, - &VescDriver::positionCallback, this); - servo_sub_ = private_nh.subscribe("commands/servo/position", 10, - &VescDriver::servoCallback, this); + duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, + &VescDriver::dutyCycleCallback, this); + current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this); + brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); + speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); + position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); + servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); // create a 50Hz timer, used for state machine & polling VESC telemetry timer_ = nh.createTimer(ros::Duration(1.0/50.0), &VescDriver::timerCallback, this); @@ -108,21 +109,21 @@ void VescDriver::vescPacketCallback(const boost::shared_ptr& p boost::shared_ptr values = boost::dynamic_pointer_cast(packet); - vesc_msgs::VescState state_msg; - state_msg.header.stamp = ros::Time::now(); - state_msg.voltage_input = values->v_in(); - state_msg.temperature_pcb = values->temp_pcb(); - state_msg.current_motor = values->current_motor(); - state_msg.current_input = values->current_in(); - state_msg.speed = values->rpm(); - state_msg.duty_cycle = values->duty_now(); - state_msg.charge_drawn = values->amp_hours(); - state_msg.charge_regen = values->amp_hours_charged(); - state_msg.energy_drawn = values->watt_hours(); - state_msg.energy_regen = values->watt_hours_charged(); - state_msg.displacement = values->tachometer(); - state_msg.distance_traveled = values->tachometer_abs(); - state_msg.fault_code = values->fault_code(); + vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); + state_msg->header.stamp = ros::Time::now(); + state_msg->state.voltage_input = values->v_in(); + state_msg->state.temperature_pcb = values->temp_pcb(); + state_msg->state.current_motor = values->current_motor(); + state_msg->state.current_input = values->current_in(); + state_msg->state.speed = values->rpm(); + state_msg->state.duty_cycle = values->duty_now(); + state_msg->state.charge_drawn = values->amp_hours(); + state_msg->state.charge_regen = values->amp_hours_charged(); + state_msg->state.energy_drawn = values->watt_hours(); + state_msg->state.energy_regen = values->watt_hours_charged(); + state_msg->state.displacement = values->tachometer(); + state_msg->state.distance_traveled = values->tachometer_abs(); + state_msg->state.fault_code = values->fault_code(); state_pub_.publish(state_msg); } @@ -147,12 +148,8 @@ void VescDriver::vescErrorCallback(const std::string& error) */ void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle) { - if (duty_cycle->data < -1.0 || duty_cycle->data > 1.0) { - ROS_ERROR("VESC duty cycle should be between -1 and 1, ignoring."); - return; - } if (driver_mode_ = MODE_OPERATING) { - vesc_.setDutyCycle(duty_cycle->data); + vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); } } @@ -164,7 +161,7 @@ void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) { if (driver_mode_ = MODE_OPERATING) { - vesc_.setCurrent(current->data); + vesc_.setCurrent(current_limit_.clip(current->data)); } } @@ -176,7 +173,7 @@ void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) { if (driver_mode_ = MODE_OPERATING) { - vesc_.setBrake(brake->data); + vesc_.setBrake(brake_limit_.clip(brake->data)); } } @@ -189,7 +186,7 @@ void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) { if (driver_mode_ = MODE_OPERATING) { - vesc_.setSpeed(speed->data); + vesc_.setSpeed(speed_limit_.clip(speed->data)); } } @@ -201,7 +198,7 @@ void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) { if (driver_mode_ = MODE_OPERATING) { // ROS uses radians but VESC seems to use degrees. Convert to degrees. - double position_deg = position->data * M_PI / 180.0; + double position_deg = position_limit_.clip(position->data) * M_PI / 180.0; vesc_.setPosition(position_deg); } } @@ -211,14 +208,87 @@ void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) */ void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) { - if (servo->data < 0 || servo->data > 1.0) { - ROS_ERROR("VESC servo position should be between 0 and 1, ignoring."); - return; + if (driver_mode_ = MODE_OPERATING) { + vesc_.setServo(servo_limit_.clip(servo->data)); } +} - if (driver_mode_ = MODE_OPERATING) { - vesc_.setServo(servo->data); +VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::string& str, + const boost::optional& min_lower, + const boost::optional& max_upper) : + name(str) +{ + // check if user's minimum value is outside of the range min_lower to max_upper + double param_min; + if (nh.getParam(name + "_min", param_min)) { + if (min_lower && param_min < *min_lower) { + lower = *min_lower; + ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << + ") is less than the feasible minimum (" << *min_lower << ")."); + } + else if (max_upper && param_min > *max_upper) { + lower = *max_upper; + ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << + ") is greater than the feasible maximum (" << *max_upper << ")."); + } + else { + lower = param_min; + } + } + else if (min_lower) { + lower = *min_lower; + } + + // check if the uers' maximum value is outside of the range min_lower to max_upper + double param_max; + if (nh.getParam(name + "_max", param_max)) { + if (min_lower && param_max < *min_lower) { + upper = *min_lower; + ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << + ") is less than the feasible minimum (" << *min_lower << ")."); + } + else if (max_upper && param_max > *max_upper) { + upper = *max_upper; + ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << + ") is greater than the feasible maximum (" << *max_upper << ")."); + } + else { + upper = param_max; + } + } + else if (max_upper) { + upper = *max_upper; } + + // check for min > max + if (upper && lower && *lower > *upper) { + ROS_WARN_STREAM("Parameter " << name << "_max (" << upper + << ") is less than parameter " << name << "_min (" << lower << ")."); + double temp(*lower); + lower = *upper; + upper = temp; + } + + std::ostringstream oss; + oss << " " << name << " limit: "; + if (lower) oss << *lower << " "; else oss << "(none) "; + if (upper) oss << *upper; else oss << "(none)"; + ROS_DEBUG_STREAM(oss.str()); } +double VescDriver::CommandLimit::clip(double value) +{ + if (lower && value < lower) { + ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", + name.c_str(), value, *lower); + value = *lower; + } + if (upper && value > upper) { + ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.", + name.c_str(), value, *upper); + value = *upper; + } +} + + } // namespace vesc_driver diff --git a/vesc_msgs/CMakeLists.txt b/vesc_msgs/CMakeLists.txt index 29699a0..d1365bd 100644 --- a/vesc_msgs/CMakeLists.txt +++ b/vesc_msgs/CMakeLists.txt @@ -10,6 +10,7 @@ add_message_files( DIRECTORY msg FILES VescState.msg + VescStateStamped.msg ) generate_messages( DEPENDENCIES diff --git a/vesc_msgs/msg/VescState.msg b/vesc_msgs/msg/VescState.msg index 055b49c..276d62a 100644 --- a/vesc_msgs/msg/VescState.msg +++ b/vesc_msgs/msg/VescState.msg @@ -9,7 +9,6 @@ int32 FAULT_CODE_ABS_OVER_CURRENT=4 int32 FAULT_CODE_OVER_TEMP_FET=5 int32 FAULT_CODE_OVER_TEMP_MOTOR=6 -Header header float64 voltage_input # input voltage (volt) float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) float64 current_motor # motor current (ampere) diff --git a/vesc_msgs/msg/VescStateStamped.msg b/vesc_msgs/msg/VescStateStamped.msg new file mode 100644 index 0000000..086d5b8 --- /dev/null +++ b/vesc_msgs/msg/VescStateStamped.msg @@ -0,0 +1,4 @@ +# Timestamped VESC open source motor controller state (telemetry) + +Header header +VescState state \ No newline at end of file