00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #include <cmath>
00041
00042 #include <tf/transform_datatypes.h>
00043
00044 #include <urdf_parser/urdf_parser.h>
00045
00046 #include <boost/assign.hpp>
00047
00048 #include <steer_drive_controller/steer_drive_controller.h>
00049
00050 static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2)
00051 {
00052 return std::sqrt(std::pow(vec1.x-vec2.x,2) +
00053 std::pow(vec1.y-vec2.y,2) +
00054 std::pow(vec1.z-vec2.z,2));
00055 }
00056
00057
00058
00059
00060
00061
00062 static bool isCylinder(const boost::shared_ptr<const urdf::Link>& link)
00063 {
00064 if (!link)
00065 {
00066 ROS_ERROR("Link == NULL.");
00067 return false;
00068 }
00069
00070 if (!link->collision)
00071 {
00072 ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
00073 return false;
00074 }
00075
00076 if (!link->collision->geometry)
00077 {
00078 ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
00079 return false;
00080 }
00081
00082 if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
00083 {
00084 ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
00085 return false;
00086 }
00087
00088 return true;
00089 }
00090
00091
00092
00093
00094
00095
00096
00097
00098 static bool getWheelRadius(const boost::shared_ptr<const urdf::Link>& wheel_link, double& wheel_radius)
00099 {
00100 if (!isCylinder(wheel_link))
00101 {
00102 ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
00103 return false;
00104 }
00105
00106 wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
00107 return true;
00108 }
00109
00110 namespace steer_drive_controller{
00111
00112 SteerDriveController::SteerDriveController()
00113 : open_loop_(false)
00114 , command_struct_()
00115 , wheel_separation_h_(0.0)
00116 , wheel_radius_(0.0)
00117 , wheel_separation_h_multiplier_(1.0)
00118 , wheel_radius_multiplier_(1.0)
00119 , steer_pos_multiplier_(1.0)
00120 , cmd_vel_timeout_(0.5)
00121 , allow_multiple_cmd_vel_publishers_(true)
00122 , base_frame_id_("base_link")
00123 , odom_frame_id_("odom")
00124 , enable_odom_tf_(true)
00125 , wheel_joints_size_(0)
00126 , ns_("")
00127 {
00128 }
00129
00130 bool SteerDriveController::init(hardware_interface::RobotHW* robot_hw,
00131 ros::NodeHandle& root_nh,
00132 ros::NodeHandle& controller_nh)
00133 {
00134 typedef hardware_interface::VelocityJointInterface VelIface;
00135 typedef hardware_interface::PositionJointInterface PosIface;
00136 typedef hardware_interface::JointStateInterface StateIface;
00137
00138
00139 VelIface *vel_joint_if = robot_hw->get<VelIface>();
00140 PosIface *pos_joint_if = robot_hw->get<PosIface>();
00141
00142 const std::string complete_ns = controller_nh.getNamespace();
00143
00144 std::size_t id = complete_ns.find_last_of("/");
00145 name_ = complete_ns.substr(id + 1);
00146
00147
00148 std::string rear_wheel_name = "rear_wheel_joint";
00149 controller_nh.param(ns_ + "rear_wheel", rear_wheel_name, rear_wheel_name);
00150
00151
00152 std::string front_steer_name = "front_steer_joint";
00153 controller_nh.param(ns_ + "front_steer", front_steer_name, front_steer_name);
00154
00155
00156
00157 double publish_rate;
00158 controller_nh.param("publish_rate", publish_rate, 50.0);
00159 ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
00160 << publish_rate << "Hz.");
00161 publish_period_ = ros::Duration(1.0 / publish_rate);
00162
00163 controller_nh.param(ns_ + "open_loop", open_loop_, open_loop_);
00164
00165 controller_nh.param(ns_ + "wheel_separation_h_multiplier", wheel_separation_h_multiplier_, wheel_separation_h_multiplier_);
00166 ROS_INFO_STREAM_NAMED(name_, "Wheel separation height will be multiplied by "
00167 << wheel_separation_h_multiplier_ << ".");
00168
00169 controller_nh.param(ns_ + "wheel_radius_multiplier", wheel_radius_multiplier_, wheel_radius_multiplier_);
00170 ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by "
00171 << wheel_radius_multiplier_ << ".");
00172
00173 controller_nh.param(ns_ + "steer_pos_multiplier", steer_pos_multiplier_, steer_pos_multiplier_);
00174 ROS_INFO_STREAM_NAMED(name_, "Steer pos will be multiplied by "
00175 << steer_pos_multiplier_ << ".");
00176
00177 int velocity_rolling_window_size = 10;
00178 controller_nh.param(ns_ + "velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
00179 ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of "
00180 << velocity_rolling_window_size << ".");
00181
00182 odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
00183
00184
00185 controller_nh.param(ns_ + "cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
00186 ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
00187 << cmd_vel_timeout_ << "s.");
00188
00189 controller_nh.param(ns_ + "allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_);
00190 ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is "
00191 << (allow_multiple_cmd_vel_publishers_?"enabled":"disabled"));
00192
00193 controller_nh.param(ns_ + "base_frame_id", base_frame_id_, base_frame_id_);
00194 ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
00195
00196 controller_nh.param(ns_ + "odom_frame_id", odom_frame_id_, odom_frame_id_);
00197 ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
00198
00199 controller_nh.param(ns_ + "enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
00200 ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));
00201
00202
00203 controller_nh.param(ns_ + "linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits );
00204 controller_nh.param(ns_ + "linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
00205 controller_nh.param(ns_ + "linear/x/has_jerk_limits" , limiter_lin_.has_jerk_limits , limiter_lin_.has_jerk_limits );
00206 controller_nh.param(ns_ + "linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity );
00207 controller_nh.param(ns_ + "linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity );
00208 controller_nh.param(ns_ + "linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration );
00209 controller_nh.param(ns_ + "linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration );
00210 controller_nh.param(ns_ + "linear/x/max_jerk" , limiter_lin_.max_jerk , limiter_lin_.max_jerk );
00211 controller_nh.param(ns_ + "linear/x/min_jerk" , limiter_lin_.min_jerk , -limiter_lin_.max_jerk );
00212
00213 controller_nh.param(ns_ + "angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits );
00214 controller_nh.param(ns_ + "angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
00215 controller_nh.param(ns_ + "angular/z/has_jerk_limits" , limiter_ang_.has_jerk_limits , limiter_ang_.has_jerk_limits );
00216 controller_nh.param(ns_ + "angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity );
00217 controller_nh.param(ns_ + "angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
00218 controller_nh.param(ns_ + "angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
00219 controller_nh.param(ns_ + "angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
00220 controller_nh.param(ns_ + "angular/z/max_jerk" , limiter_ang_.max_jerk , limiter_ang_.max_jerk );
00221 controller_nh.param(ns_ + "angular/z/min_jerk" , limiter_ang_.min_jerk , -limiter_ang_.max_jerk );
00222
00223
00224 bool lookup_wheel_separation_h = !controller_nh.getParam(ns_ + "wheel_separation_h", wheel_separation_h_);
00225 bool lookup_wheel_radius = !controller_nh.getParam(ns_ + "wheel_radius", wheel_radius_);
00226
00227 if (!setOdomParamsFromUrdf(root_nh,
00228 rear_wheel_name,
00229 front_steer_name,
00230 lookup_wheel_separation_h,
00231 lookup_wheel_radius))
00232 {
00233 return false;
00234 }
00235
00236
00237
00238 const double ws_h = wheel_separation_h_multiplier_ * wheel_separation_h_;
00239 const double wr = wheel_radius_multiplier_ * wheel_radius_;
00240 odometry_.setWheelParams(ws_h, wr);
00241 ROS_INFO_STREAM_NAMED(name_,
00242 "Odometry params : wheel separation height " << ws_h
00243 << ", wheel radius " << wr);
00244
00245 setOdomPubFields(root_nh, controller_nh);
00246
00247
00248
00249 ROS_INFO_STREAM_NAMED(name_,
00250 "Adding the rear wheel with joint name: " << rear_wheel_name);
00251 rear_wheel_joint_ = vel_joint_if->getHandle(rear_wheel_name);
00252
00253 ROS_INFO_STREAM_NAMED(name_,
00254 "Adding the front steer with joint name: " << front_steer_name);
00255 front_steer_joint_ = pos_joint_if->getHandle(front_steer_name);
00256 ROS_INFO_STREAM_NAMED(name_,
00257 "Adding the subscriber: cmd_vel");
00258 sub_command_ = controller_nh.subscribe("cmd_vel", 1, &SteerDriveController::cmdVelCallback, this);
00259 ROS_INFO_STREAM_NAMED(name_, "Finished controller initialization");
00260
00261 return true;
00262 }
00263
00264 void SteerDriveController::update(const ros::Time& time, const ros::Duration& period)
00265 {
00266
00267 if (open_loop_)
00268 {
00269 odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
00270 }
00271 else
00272 {
00273 double wheel_pos = rear_wheel_joint_.getPosition();
00274 double steer_pos = front_steer_joint_.getPosition();
00275
00276 if (std::isnan(wheel_pos) || std::isnan(steer_pos))
00277 return;
00278
00279
00280 steer_pos = steer_pos * steer_pos_multiplier_;
00281 odometry_.update(wheel_pos, steer_pos, time);
00282 }
00283
00284
00285 if (last_state_publish_time_ + publish_period_ < time)
00286 {
00287 last_state_publish_time_ += publish_period_;
00288
00289 const geometry_msgs::Quaternion orientation(
00290 tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
00291
00292
00293 if (odom_pub_->trylock())
00294 {
00295 odom_pub_->msg_.header.stamp = time;
00296 odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
00297 odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
00298 odom_pub_->msg_.pose.pose.orientation = orientation;
00299 odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
00300 odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
00301 odom_pub_->unlockAndPublish();
00302 }
00303
00304
00305 if (enable_odom_tf_ && tf_odom_pub_->trylock())
00306 {
00307 geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
00308 odom_frame.header.stamp = time;
00309 odom_frame.transform.translation.x = odometry_.getX();
00310 odom_frame.transform.translation.y = odometry_.getY();
00311 odom_frame.transform.rotation = orientation;
00312 tf_odom_pub_->unlockAndPublish();
00313 }
00314 }
00315
00316
00317
00318 Commands curr_cmd = *(command_.readFromRT());
00319 const double dt = (time - curr_cmd.stamp).toSec();
00320
00321
00322 if (dt > cmd_vel_timeout_)
00323 {
00324 curr_cmd.lin = 0.0;
00325 curr_cmd.ang = 0.0;
00326 }
00327
00328
00329 const double cmd_dt(period.toSec());
00330
00331 limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
00332 limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
00333
00334 last1_cmd_ = last0_cmd_;
00335 last0_cmd_ = curr_cmd;
00336
00337
00338 const double wheel_vel = curr_cmd.lin/wheel_radius_;
00339 rear_wheel_joint_.setCommand(wheel_vel);
00340 front_steer_joint_.setCommand(curr_cmd.ang);
00341
00342 }
00343
00344 void SteerDriveController::starting(const ros::Time& time)
00345 {
00346 brake();
00347
00348
00349 last_state_publish_time_ = time;
00350
00351 odometry_.init(time);
00352 }
00353
00354 void SteerDriveController::stopping(const ros::Time& )
00355 {
00356 brake();
00357 }
00358
00359 void SteerDriveController::brake()
00360 {
00361 const double steer_pos = 0.0;
00362 const double wheel_vel = 0.0;
00363
00364 rear_wheel_joint_.setCommand(steer_pos);
00365 front_steer_joint_.setCommand(wheel_vel);
00366 }
00367
00368 void SteerDriveController::cmdVelCallback(const geometry_msgs::Twist& command)
00369 {
00370 if (isRunning())
00371 {
00372
00373 if (!allow_multiple_cmd_vel_publishers_ && sub_command_.getNumPublishers() > 1)
00374 {
00375 ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers()
00376 << " publishers. Only 1 publisher is allowed. Going to brake.");
00377 brake();
00378 return;
00379 }
00380
00381 command_struct_.ang = command.angular.z;
00382 command_struct_.lin = command.linear.x;
00383 command_struct_.stamp = ros::Time::now();
00384 command_.writeFromNonRT (command_struct_);
00385 ROS_DEBUG_STREAM_NAMED(name_,
00386 "Added values to command. "
00387 << "Ang: " << command_struct_.ang << ", "
00388 << "Lin: " << command_struct_.lin << ", "
00389 << "Stamp: " << command_struct_.stamp);
00390 }
00391 else
00392 {
00393 ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
00394 }
00395 }
00396
00397
00398 bool SteerDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
00399 const std::string rear_wheel_name,
00400 const std::string front_steer_name,
00401 bool lookup_wheel_separation_h,
00402 bool lookup_wheel_radius)
00403 {
00404 if (!(lookup_wheel_separation_h || lookup_wheel_radius))
00405 {
00406
00407 return true;
00408 }
00409
00410
00411 const std::string model_param_name = "robot_description";
00412 bool res = root_nh.hasParam(model_param_name);
00413 std::string robot_model_str="";
00414 if (!res || !root_nh.getParam(model_param_name,robot_model_str))
00415 {
00416 ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
00417 return false;
00418 }
00419
00420 boost::shared_ptr<urdf::ModelInterface> model(urdf::parseURDF(robot_model_str));
00421
00422 boost::shared_ptr<const urdf::Joint> rear_wheel_joint(model->getJoint(rear_wheel_name));
00423 boost::shared_ptr<const urdf::Joint> front_steer_joint(model->getJoint(front_steer_name));
00424
00425 if (lookup_wheel_separation_h)
00426 {
00427
00428 if (!rear_wheel_joint)
00429 {
00430 ROS_ERROR_STREAM_NAMED(name_, rear_wheel_name
00431 << " couldn't be retrieved from model description");
00432 return false;
00433 }
00434
00435 if (!front_steer_joint)
00436 {
00437 ROS_ERROR_STREAM_NAMED(name_, front_steer_name
00438 << " couldn't be retrieved from model description");
00439 return false;
00440 }
00441
00442 ROS_INFO_STREAM("rear wheel to origin: "
00443 << rear_wheel_joint->parent_to_joint_origin_transform.position.x << ","
00444 << rear_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
00445 << rear_wheel_joint->parent_to_joint_origin_transform.position.z);
00446
00447 ROS_INFO_STREAM("front steer to origin: "
00448 << front_steer_joint->parent_to_joint_origin_transform.position.x << ","
00449 << front_steer_joint->parent_to_joint_origin_transform.position.y << ", "
00450 << front_steer_joint->parent_to_joint_origin_transform.position.z);
00451
00452 wheel_separation_h_ = fabs(
00453 rear_wheel_joint->parent_to_joint_origin_transform.position.x
00454 - front_steer_joint->parent_to_joint_origin_transform.position.x);
00455
00456 ROS_INFO_STREAM("Calculated wheel_separation_h: " << wheel_separation_h_);
00457 }
00458
00459 if (lookup_wheel_radius)
00460 {
00461
00462 if (!getWheelRadius(model->getLink(rear_wheel_joint->child_link_name), wheel_radius_))
00463 {
00464 ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << rear_wheel_name << " wheel radius");
00465 return false;
00466 }
00467 ROS_INFO_STREAM("Retrieved wheel_radius: " << wheel_radius_);
00468 }
00469
00470 return true;
00471 }
00472
00473 void SteerDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
00474 {
00475
00476 XmlRpc::XmlRpcValue pose_cov_list;
00477 controller_nh.getParam(ns_ + "pose_covariance_diagonal", pose_cov_list);
00478 ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00479 ROS_ASSERT(pose_cov_list.size() == 6);
00480 for (int i = 0; i < pose_cov_list.size(); ++i)
00481 ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00482
00483 XmlRpc::XmlRpcValue twist_cov_list;
00484 controller_nh.getParam(ns_ + "twist_covariance_diagonal", twist_cov_list);
00485 ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00486 ROS_ASSERT(twist_cov_list.size() == 6);
00487 for (int i = 0; i < twist_cov_list.size(); ++i)
00488 ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00489
00490
00491 odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, ns_ + "odom", 100));
00492 odom_pub_->msg_.header.frame_id = odom_frame_id_;
00493 odom_pub_->msg_.child_frame_id = base_frame_id_;
00494 odom_pub_->msg_.pose.pose.position.z = 0;
00495 odom_pub_->msg_.pose.covariance = boost::assign::list_of
00496 (static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
00497 (0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
00498 (0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
00499 (0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
00500 (0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
00501 (0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
00502 odom_pub_->msg_.twist.twist.linear.y = 0;
00503 odom_pub_->msg_.twist.twist.linear.z = 0;
00504 odom_pub_->msg_.twist.twist.angular.x = 0;
00505 odom_pub_->msg_.twist.twist.angular.y = 0;
00506 odom_pub_->msg_.twist.covariance = boost::assign::list_of
00507 (static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
00508 (0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
00509 (0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
00510 (0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
00511 (0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
00512 (0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
00513 tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
00514 tf_odom_pub_->msg_.transforms.resize(1);
00515 tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
00516 tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
00517 tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
00518 }
00519
00520 }