43 #include <urdf_parser/urdf_parser.h> 45 #include <urdf/urdfdom_compatibility.h> 47 #include <boost/assign.hpp> 53 return std::sqrt(std::pow(vec1.x-vec2.x,2) +
54 std::pow(vec1.y-vec2.y,2) +
55 std::pow(vec1.z-vec2.z,2));
73 ROS_ERROR_STREAM(
"Link " << link->name <<
" does not have collision description. Add collision description for link to urdf.");
77 if (!link->collision->geometry)
79 ROS_ERROR_STREAM(
"Link " << link->name <<
" does not have collision geometry description. Add collision geometry description for link to urdf.");
90 static bool isCylinder(
const urdf::LinkConstSharedPtr& link)
97 if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
99 ROS_DEBUG_STREAM(
"Link " << link->name <<
" does not have cylinder geometry");
111 static bool isSphere(
const urdf::LinkConstSharedPtr& link)
118 if (link->collision->geometry->type != urdf::Geometry::SPHERE)
120 ROS_DEBUG_STREAM(
"Link " << link->name <<
" does not have sphere geometry");
133 static bool getWheelRadius(
const urdf::LinkConstSharedPtr& wheel_link,
double& wheel_radius)
137 wheel_radius = (
static_cast<urdf::Cylinder*
>(wheel_link->collision->geometry.get()))->radius;
142 wheel_radius = (
static_cast<urdf::Sphere*
>(wheel_link->collision->geometry.get()))->radius;
146 ROS_ERROR_STREAM(
"Wheel link " << wheel_link->name <<
" is NOT modeled as a cylinder or sphere!");
155 , wheel_separation_(0.0)
157 , wheel_separation_multiplier_(1.0)
158 , left_wheel_radius_multiplier_(1.0)
159 , right_wheel_radius_multiplier_(1.0)
160 , cmd_vel_timeout_(0.5)
161 , allow_multiple_cmd_vel_publishers_(true)
162 , base_frame_id_(
"base_link")
163 , odom_frame_id_(
"odom")
164 , enable_odom_tf_(true)
165 , wheel_joints_size_(0)
166 , publish_cmd_(false)
174 const std::string complete_ns = controller_nh.
getNamespace();
175 std::size_t
id = complete_ns.find_last_of(
"/");
176 name_ = complete_ns.substr(
id + 1);
179 std::vector<std::string> left_wheel_names, right_wheel_names;
180 if (!
getWheelNames(controller_nh,
"left_wheel", left_wheel_names) or
181 !
getWheelNames(controller_nh,
"right_wheel", right_wheel_names))
186 if (left_wheel_names.size() != right_wheel_names.size())
189 "#left wheels (" << left_wheel_names.size() <<
") != " <<
190 "#right wheels (" << right_wheel_names.size() <<
").");
203 controller_nh.
param(
"publish_rate", publish_rate, 50.0);
205 << publish_rate <<
"Hz.");
214 if (controller_nh.
hasParam(
"wheel_radius_multiplier"))
216 double wheel_radius_multiplier;
217 controller_nh.
getParam(
"wheel_radius_multiplier", wheel_radius_multiplier);
233 int velocity_rolling_window_size = 10;
234 controller_nh.
param(
"velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
236 << velocity_rolling_window_size <<
".");
288 right_wheel_names[0],
289 lookup_wheel_separation,
290 lookup_wheel_radius))
302 "Odometry params : wheel separation " << ws
303 <<
", left wheel radius " << lwr
304 <<
", right wheel radius " << rwr);
317 "Adding left wheel with joint name: " << left_wheel_names[i]
318 <<
" and right wheel with joint name: " << right_wheel_names[i]);
337 DiffDriveControllerConfig config;
342 config.publish_rate = publish_rate;
371 double left_pos = 0.0;
372 double right_pos = 0.0;
377 if (std::isnan(lp) || std::isnan(rp))
395 const geometry_msgs::Quaternion orientation(
404 odom_pub_->msg_.pose.pose.orientation = orientation;
413 geometry_msgs::TransformStamped& odom_frame =
tf_odom_pub_->msg_.transforms[0];
414 odom_frame.header.stamp = time;
417 odom_frame.transform.rotation = orientation;
425 const double dt = (time - curr_cmd.
stamp).toSec();
435 const double cmd_dt(period.
toSec());
453 const double vel_left = (curr_cmd.
lin - curr_cmd.
ang * ws / 2.0)/lwr;
454 const double vel_right = (curr_cmd.
lin + curr_cmd.
ang * ws / 2.0)/rwr;
481 const double vel = 0.0;
497 <<
" publishers. Only 1 publisher is allowed. Going to brake.");
507 "Added values to command. " 519 const std::string& wheel_param,
520 std::vector<std::string>& wheel_names)
523 if (!controller_nh.
getParam(wheel_param, wheel_list))
526 "Couldn't retrieve wheel param '" << wheel_param <<
"'.");
532 if (wheel_list.
size() == 0)
535 "Wheel param '" << wheel_param <<
"' is an empty list");
539 for (
int i = 0; i < wheel_list.
size(); ++i)
544 "Wheel param '" << wheel_param <<
"' #" << i <<
550 wheel_names.resize(wheel_list.
size());
551 for (
int i = 0; i < wheel_list.
size(); ++i)
553 wheel_names[i] =
static_cast<std::string
>(wheel_list[i]);
558 wheel_names.push_back(wheel_list);
563 "Wheel param '" << wheel_param <<
564 "' is neither a list of strings nor a string.");
572 const std::string& left_wheel_name,
573 const std::string& right_wheel_name,
574 bool lookup_wheel_separation,
575 bool lookup_wheel_radius)
577 if (!(lookup_wheel_separation || lookup_wheel_radius))
584 const std::string model_param_name =
"robot_description";
585 bool res = root_nh.
hasParam(model_param_name);
586 std::string robot_model_str=
"";
587 if (!res || !root_nh.
getParam(model_param_name,robot_model_str))
593 urdf::ModelInterfaceSharedPtr model(urdf::parseURDF(robot_model_str));
595 urdf::JointConstSharedPtr left_wheel_joint(model->getJoint(left_wheel_name));
596 urdf::JointConstSharedPtr right_wheel_joint(model->getJoint(right_wheel_name));
598 if (lookup_wheel_separation)
601 if (!left_wheel_joint)
604 <<
" couldn't be retrieved from model description");
608 if (!right_wheel_joint)
611 <<
" couldn't be retrieved from model description");
615 ROS_INFO_STREAM(
"left wheel to origin: " << left_wheel_joint->parent_to_joint_origin_transform.position.x <<
"," 616 << left_wheel_joint->parent_to_joint_origin_transform.position.y <<
", " 617 << left_wheel_joint->parent_to_joint_origin_transform.position.z);
618 ROS_INFO_STREAM(
"right wheel to origin: " << right_wheel_joint->parent_to_joint_origin_transform.position.x <<
"," 619 << right_wheel_joint->parent_to_joint_origin_transform.position.y <<
", " 620 << right_wheel_joint->parent_to_joint_origin_transform.position.z);
623 right_wheel_joint->parent_to_joint_origin_transform.position);
627 if (lookup_wheel_radius)
644 controller_nh.
getParam(
"pose_covariance_diagonal", pose_cov_list);
647 for (
int i = 0; i < pose_cov_list.
size(); ++i)
651 controller_nh.
getParam(
"twist_covariance_diagonal", twist_cov_list);
654 for (
int i = 0; i < twist_cov_list.
size(); ++i)
661 odom_pub_->msg_.pose.pose.position.z = 0;
662 odom_pub_->msg_.pose.covariance = boost::assign::list_of
663 (static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
664 (0) (
static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
665 (0) (0) (
static_cast<double>(pose_cov_list[2])) (0) (0) (0)
666 (0) (0) (0) (
static_cast<double>(pose_cov_list[3])) (0) (0)
667 (0) (0) (0) (0) (
static_cast<double>(pose_cov_list[4])) (0)
668 (0) (0) (0) (0) (0) (
static_cast<double>(pose_cov_list[5]));
669 odom_pub_->msg_.twist.twist.linear.y = 0;
670 odom_pub_->msg_.twist.twist.linear.z = 0;
671 odom_pub_->msg_.twist.twist.angular.x = 0;
672 odom_pub_->msg_.twist.twist.angular.y = 0;
673 odom_pub_->msg_.twist.covariance = boost::assign::list_of
674 (static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
675 (0) (
static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
676 (0) (0) (
static_cast<double>(twist_cov_list[2])) (0) (0) (0)
677 (0) (0) (0) (
static_cast<double>(twist_cov_list[3])) (0) (0)
678 (0) (0) (0) (0) (
static_cast<double>(twist_cov_list[4])) (0)
679 (0) (0) (0) (0) (0) (
static_cast<double>(twist_cov_list[5]));
682 tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
realtime_tools::RealtimeBuffer< Commands > command_
bool allow_multiple_cmd_vel_publishers_
Whether to allow multiple publishers on cmd_vel topic or not:
bool init(hardware_interface::VelocityJointInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Initialize controller.
double wheel_separation_
Wheel separation, wrt the midpoint of the wheel width:
void updateDynamicParams()
Update the dynamic parameters in the RT loop.
double wheel_separation_multiplier
std::vector< hardware_interface::JointHandle > left_wheel_joints_
Hardware handles:
static bool hasCollisionGeometry(const urdf::LinkConstSharedPtr &link)
Velocity command related:
bool update(double left_pos, double right_pos, const ros::Time &time)
Updates the odometry class with latest wheels position.
#define ROS_DEBUG_STREAM_NAMED(name, args)
double wheel_radius_
Wheel radius (assuming it's the same for the left and right wheels):
#define ROS_ERROR_STREAM_NAMED(name, args)
static bool getWheelRadius(const urdf::LinkConstSharedPtr &wheel_link, double &wheel_radius)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool has_acceleration_limits
std::string odom_frame_id_
Frame to use for odometry and odom tf:
static bool isCylinder(const urdf::LinkConstSharedPtr &link)
void brake()
Brakes the wheels, i.e. sets the velocity to 0.
size_t wheel_joints_size_
Number of wheel joints:
#define ROS_INFO_STREAM_NAMED(name, args)
bool getWheelNames(ros::NodeHandle &controller_nh, const std::string &wheel_param, std::vector< std::string > &wheel_names)
Get the wheel names from a wheel param.
double getHeading() const
heading getter
Type const & getType() const
double right_wheel_radius_multiplier
double getY() const
y position getter
static bool isSphere(const urdf::LinkConstSharedPtr &link)
std::vector< hardware_interface::JointHandle > right_wheel_joints_
void starting(const ros::Time &time)
Starts controller.
double getAngular() const
angular velocity getter
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
double getX() const
x position getter
boost::shared_ptr< ReconfigureServer > dyn_reconf_server_
void init(const ros::Time &time)
Initialize the odometry.
void cmdVelCallback(const geometry_msgs::Twist &command)
Velocity command callback.
uint32_t getNumPublishers() const
ros::Time last_state_publish_time_
ros::Subscriber sub_command_
void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius)
Sets the wheel parameters: radius and separation.
const std::string & getNamespace() const
void update(const ros::Time &time, const ros::Duration &period)
Updates controller, i.e. computes the odometry and sets the new velocity commands.
realtime_tools::RealtimeBuffer< DynamicParams > dynamic_params_
#define ROS_DEBUG_STREAM(args)
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
Velocity rolling window size setter.
double right_wheel_radius_multiplier_
ros::Duration publish_period_
Odometry related:
JointHandle getHandle(const std::string &name)
double left_wheel_radius_multiplier
boost::shared_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > > odom_pub_
Odometry related:
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
#define ROS_INFO_STREAM(args)
static double euclideanOfVectors(const urdf::Vector3 &vec1, const urdf::Vector3 &vec2)
bool publish_cmd_
Publish limited velocity:
SpeedLimiter limiter_lin_
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM_THROTTLE_NAMED(rate, name, args)
double getLinear() const
linear velocity getter
void updateOpenLoop(double linear, double angular, const ros::Time &time)
Updates the odometry class with latest velocity command.
double cmd_vel_timeout_
Timeout to consider cmd_vel commands old:
boost::shared_ptr< realtime_tools::RealtimePublisher< tf::tfMessage > > tf_odom_pub_
#define ROS_ERROR_NAMED(name,...)
SpeedLimiter limiter_ang_
bool setOdomParamsFromUrdf(ros::NodeHandle &root_nh, const std::string &left_wheel_name, const std::string &right_wheel_name, bool lookup_wheel_separation, bool lookup_wheel_radius)
Sets odometry parameters from the URDF, i.e. the wheel radius and separation.
double left_wheel_radius_multiplier_
bool enable_odom_tf_
Whether to publish odometry to tf or not:
Commands last1_cmd_
Speed limiters:
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
void reconfCallback(DiffDriveControllerConfig &config, uint32_t)
Callback for dynamic_reconfigure server.
boost::shared_ptr< realtime_tools::RealtimePublisher< geometry_msgs::TwistStamped > > cmd_vel_pub_
Publish executed commands.
void setOdomPubFields(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Sets the odometry publishing fields.
double limit(double &v, double v0, double v1, double dt)
Limit the velocity and acceleration.
void stopping(const ros::Time &)
Stops controller.
double wheel_separation_multiplier_
Wheel separation and radius calibration multipliers:
std::string base_frame_id_
Frame to use for the robot base: