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 #include <cmath>
00040
00041 #include <tf/transform_datatypes.h>
00042
00043 #include <urdf_parser/urdf_parser.h>
00044
00045 #include <boost/assign.hpp>
00046
00047 #include <robotican_controllers/diff_drive_controller.h>
00048
00049 static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2)
00050 {
00051 return std::sqrt(std::pow(vec1.x-vec2.x,2) +
00052 std::pow(vec1.y-vec2.y,2) +
00053 std::pow(vec1.z-vec2.z,2));
00054 }
00055
00056
00057
00058
00059
00060
00061 static bool isCylinder(const boost::shared_ptr<const urdf::Link>& link)
00062 {
00063 if (!link)
00064 {
00065 ROS_ERROR("Link == NULL.");
00066 return false;
00067 }
00068
00069 if (!link->collision)
00070 {
00071 ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
00072 return false;
00073 }
00074
00075 if (!link->collision->geometry)
00076 {
00077 ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
00078 return false;
00079 }
00080
00081 if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
00082 {
00083 ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
00084 return false;
00085 }
00086
00087 return true;
00088 }
00089
00090
00091
00092
00093
00094
00095
00096 static bool getWheelRadius(const boost::shared_ptr<const urdf::Link>& wheel_link, double& wheel_radius)
00097 {
00098 if (!isCylinder(wheel_link))
00099 {
00100 ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
00101 return false;
00102 }
00103
00104 wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
00105 return true;
00106 }
00107
00108 namespace diff_drive_controller{
00109
00110 DiffDriveController::DiffDriveController()
00111 : open_loop_(false)
00112 , command_struct_()
00113 , wheel_separation_(0.0)
00114 , wheel_radius_(0.0)
00115 , wheel_separation_multiplier_(1.0)
00116 , wheel_radius_multiplier_(1.0)
00117 , cmd_vel_timeout_(0.5)
00118 , allow_multiple_cmd_vel_publishers_(true)
00119 , base_frame_id_("base_link")
00120 , odom_frame_id_("odom")
00121 , enable_odom_tf_(true)
00122 , wheel_joints_size_(0), odometry_(0, 0) {
00123 }
00124
00125 bool DiffDriveController::init(hardware_interface::VelocityJointInterface* hw,
00126 ros::NodeHandle& root_nh,
00127 ros::NodeHandle &controller_nh)
00128 {
00129 const std::string complete_ns = controller_nh.getNamespace();
00130 std::size_t id = complete_ns.find_last_of("/");
00131 name_ = complete_ns.substr(id + 1);
00132
00133
00134 std::vector<std::string> left_wheel_names, right_wheel_names;
00135 if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) or
00136 !getWheelNames(controller_nh, "right_wheel", right_wheel_names))
00137 {
00138 return false;
00139 }
00140
00141 if (left_wheel_names.size() != right_wheel_names.size())
00142 {
00143 ROS_ERROR_STREAM_NAMED(name_,
00144 "#left wheels (" << left_wheel_names.size() << ") != " <<
00145 "#right wheels (" << right_wheel_names.size() << ").");
00146 return false;
00147 }
00148 else
00149 {
00150 wheel_joints_size_ = left_wheel_names.size();
00151
00152 left_wheel_joints_.resize(wheel_joints_size_);
00153 right_wheel_joints_.resize(wheel_joints_size_);
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("open_loop", open_loop_, open_loop_);
00164
00165 controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
00166 ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by "
00167 << wheel_separation_multiplier_ << ".");
00168
00169 controller_nh.param("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 int velocity_rolling_window_size = 10;
00174 controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
00175 ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of "
00176 << velocity_rolling_window_size << ".");
00177 odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
00178
00179 double slipFactor;
00180 controller_nh.param("slip_factor", slipFactor, 0.0);
00181 odometry_.setSlipFactor(slipFactor);
00182 ROS_INFO_STREAM_NAMED(name_, "Slip factor is " << slipFactor << ".");
00183
00184
00185 controller_nh.param("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("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("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("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("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("linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits );
00204 controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
00205 controller_nh.param("linear/x/has_jerk_limits" , limiter_lin_.has_jerk_limits , limiter_lin_.has_jerk_limits );
00206 controller_nh.param("linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity );
00207 controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity );
00208 controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration );
00209 controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration );
00210 controller_nh.param("linear/x/max_jerk" , limiter_lin_.max_jerk , limiter_lin_.max_jerk );
00211 controller_nh.param("linear/x/min_jerk" , limiter_lin_.min_jerk , -limiter_lin_.max_jerk );
00212
00213 controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits );
00214 controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
00215 controller_nh.param("angular/z/has_jerk_limits" , limiter_ang_.has_jerk_limits , limiter_ang_.has_jerk_limits );
00216 controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity );
00217 controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
00218 controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
00219 controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
00220 controller_nh.param("angular/z/max_jerk" , limiter_ang_.max_jerk , limiter_ang_.max_jerk );
00221 controller_nh.param("angular/z/min_jerk" , limiter_ang_.min_jerk , -limiter_ang_.max_jerk );
00222
00223
00224 bool lookup_wheel_separation = !controller_nh.getParam("wheel_separation", wheel_separation_);
00225 bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
00226
00227 if (!setOdomParamsFromUrdf(root_nh,
00228 left_wheel_names[0],
00229 right_wheel_names[0],
00230 lookup_wheel_separation,
00231 lookup_wheel_radius))
00232 {
00233 return false;
00234 }
00235
00236
00237
00238 const double ws = wheel_separation_multiplier_ * wheel_separation_;
00239 const double wr = wheel_radius_multiplier_ * wheel_radius_;
00240 odometry_.setWheelParams(ws, wr);
00241 ROS_INFO_STREAM_NAMED(name_,
00242 "Odometry params : wheel separation " << ws
00243 << ", wheel radius " << wr);
00244
00245 setOdomPubFields(root_nh, controller_nh);
00246
00247
00248 for (int i = 0; i < wheel_joints_size_; ++i)
00249 {
00250 ROS_INFO_STREAM_NAMED(name_,
00251 "Adding left wheel with joint name: " << left_wheel_names[i]
00252 << " and right wheel with joint name: " << right_wheel_names[i]);
00253 left_wheel_joints_[i] = hw->getHandle(left_wheel_names[i]);
00254 right_wheel_joints_[i] = hw->getHandle(right_wheel_names[i]);
00255 }
00256
00257 sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this);
00258
00259 return true;
00260 }
00261
00262 void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
00263 {
00264
00265 if (open_loop_)
00266 {
00267 odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
00268 }
00269 else
00270 {
00271 double left_pos = 0.0;
00272 double right_pos = 0.0;
00273 for (size_t i = 0; i < wheel_joints_size_; ++i)
00274 {
00275 const double lp = left_wheel_joints_[i].getPosition();
00276 const double rp = right_wheel_joints_[i].getPosition();
00277 if (std::isnan(lp) || std::isnan(rp))
00278 return;
00279
00280 left_pos += lp;
00281 right_pos += rp;
00282 }
00283 left_pos /= wheel_joints_size_;
00284 right_pos /= wheel_joints_size_;
00285
00286
00287 odometry_.update(left_pos, right_pos, time);
00288 }
00289
00290
00291 if (last_state_publish_time_ + publish_period_ < time)
00292 {
00293 last_state_publish_time_ += publish_period_;
00294
00295 const geometry_msgs::Quaternion orientation(
00296 tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
00297
00298
00299 if (odom_pub_->trylock())
00300 {
00301 odom_pub_->msg_.header.stamp = time;
00302 odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
00303 odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
00304 odom_pub_->msg_.pose.pose.orientation = orientation;
00305 odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
00306 odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
00307 odom_pub_->unlockAndPublish();
00308 }
00309
00310
00311 if (enable_odom_tf_ && tf_odom_pub_->trylock())
00312 {
00313 geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
00314 odom_frame.header.stamp = time;
00315 odom_frame.transform.translation.x = odometry_.getX();
00316 odom_frame.transform.translation.y = odometry_.getY();
00317 odom_frame.transform.rotation = orientation;
00318 tf_odom_pub_->unlockAndPublish();
00319 }
00320 }
00321
00322
00323
00324 Commands curr_cmd = *(command_.readFromRT());
00325 const double dt = (time - curr_cmd.stamp).toSec();
00326
00327
00328 if (dt > cmd_vel_timeout_)
00329 {
00330 curr_cmd.lin = 0.0;
00331 curr_cmd.ang = 0.0;
00332 }
00333
00334
00335 const double cmd_dt(period.toSec());
00336
00337 limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
00338 limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
00339
00340 last1_cmd_ = last0_cmd_;
00341 last0_cmd_ = curr_cmd;
00342
00343
00344 const double ws = wheel_separation_multiplier_ * wheel_separation_;
00345 const double wr = wheel_radius_multiplier_ * wheel_radius_;
00346
00347
00348 const double vel_left = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wr;
00349 const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wr;
00350
00351
00352 for (size_t i = 0; i < wheel_joints_size_; ++i)
00353 {
00354 left_wheel_joints_[i].setCommand(vel_left);
00355 right_wheel_joints_[i].setCommand(vel_right);
00356 }
00357 }
00358
00359 void DiffDriveController::starting(const ros::Time& time)
00360 {
00361 brake();
00362
00363
00364 last_state_publish_time_ = time;
00365
00366 odometry_.init(time);
00367 }
00368
00369 void DiffDriveController::stopping(const ros::Time& )
00370 {
00371 brake();
00372 }
00373
00374 void DiffDriveController::brake()
00375 {
00376 const double vel = 0.0;
00377 for (size_t i = 0; i < wheel_joints_size_; ++i)
00378 {
00379 left_wheel_joints_[i].setCommand(vel);
00380 right_wheel_joints_[i].setCommand(vel);
00381 }
00382 }
00383
00384 void DiffDriveController::cmdVelCallback(const geometry_msgs::Twist& command)
00385 {
00386 if (isRunning())
00387 {
00388
00389 if (!allow_multiple_cmd_vel_publishers_ && sub_command_.getNumPublishers() > 1)
00390 {
00391 ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers()
00392 << " publishers. Only 1 publisher is allowed. Going to brake.");
00393 brake();
00394 return;
00395 }
00396
00397 command_struct_.ang = command.angular.z/odometry_.getSlipFactor();
00398 command_struct_.lin = command.linear.x;
00399 command_struct_.stamp = ros::Time::now();
00400 command_.writeFromNonRT (command_struct_);
00401 ROS_DEBUG_STREAM_NAMED(name_,
00402 "Added values to command. "
00403 << "Ang: " << command_struct_.ang << ", "
00404 << "Lin: " << command_struct_.lin << ", "
00405 << "Stamp: " << command_struct_.stamp);
00406 }
00407 else
00408 {
00409 ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
00410 }
00411 }
00412
00413 bool DiffDriveController::getWheelNames(ros::NodeHandle& controller_nh,
00414 const std::string& wheel_param,
00415 std::vector<std::string>& wheel_names)
00416 {
00417 XmlRpc::XmlRpcValue wheel_list;
00418 if (!controller_nh.getParam(wheel_param, wheel_list))
00419 {
00420 ROS_ERROR_STREAM_NAMED(name_,
00421 "Couldn't retrieve wheel param '" << wheel_param << "'.");
00422 return false;
00423 }
00424
00425 if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
00426 {
00427 if (wheel_list.size() == 0)
00428 {
00429 ROS_ERROR_STREAM_NAMED(name_,
00430 "Wheel param '" << wheel_param << "' is an empty list");
00431 return false;
00432 }
00433
00434 for (int i = 0; i < wheel_list.size(); ++i)
00435 {
00436 if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00437 {
00438 ROS_ERROR_STREAM_NAMED(name_,
00439 "Wheel param '" << wheel_param << "' #" << i <<
00440 " isn't a string.");
00441 return false;
00442 }
00443 }
00444
00445 wheel_names.resize(wheel_list.size());
00446 for (int i = 0; i < wheel_list.size(); ++i)
00447 {
00448 wheel_names[i] = static_cast<std::string>(wheel_list[i]);
00449 }
00450 }
00451 else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
00452 {
00453 wheel_names.push_back(wheel_list);
00454 }
00455 else
00456 {
00457 ROS_ERROR_STREAM_NAMED(name_,
00458 "Wheel param '" << wheel_param <<
00459 "' is neither a list of strings nor a string.");
00460 return false;
00461 }
00462
00463 return true;
00464 }
00465
00466 bool DiffDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
00467 const std::string& left_wheel_name,
00468 const std::string& right_wheel_name,
00469 bool lookup_wheel_separation,
00470 bool lookup_wheel_radius)
00471 {
00472 if (!(lookup_wheel_separation || lookup_wheel_radius))
00473 {
00474
00475 return true;
00476 }
00477
00478
00479 const std::string model_param_name = "robot_description";
00480 bool res = root_nh.hasParam(model_param_name);
00481 std::string robot_model_str="";
00482 if (!res || !root_nh.getParam(model_param_name,robot_model_str))
00483 {
00484 ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
00485 return false;
00486 }
00487
00488 boost::shared_ptr<urdf::ModelInterface> model(urdf::parseURDF(robot_model_str));
00489
00490 boost::shared_ptr<const urdf::Joint> left_wheel_joint(model->getJoint(left_wheel_name));
00491 boost::shared_ptr<const urdf::Joint> right_wheel_joint(model->getJoint(right_wheel_name));
00492
00493 if (lookup_wheel_separation)
00494 {
00495
00496 if (!left_wheel_joint)
00497 {
00498 ROS_ERROR_STREAM_NAMED(name_, left_wheel_name
00499 << " couldn't be retrieved from model description");
00500 return false;
00501 }
00502
00503 if (!right_wheel_joint)
00504 {
00505 ROS_ERROR_STREAM_NAMED(name_, right_wheel_name
00506 << " couldn't be retrieved from model description");
00507 return false;
00508 }
00509
00510 ROS_INFO_STREAM("left wheel to origin: " << left_wheel_joint->parent_to_joint_origin_transform.position.x << ","
00511 << left_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
00512 << left_wheel_joint->parent_to_joint_origin_transform.position.z);
00513 ROS_INFO_STREAM("right wheel to origin: " << right_wheel_joint->parent_to_joint_origin_transform.position.x << ","
00514 << right_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
00515 << right_wheel_joint->parent_to_joint_origin_transform.position.z);
00516
00517 wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position,
00518 right_wheel_joint->parent_to_joint_origin_transform.position);
00519
00520 }
00521
00522 if (lookup_wheel_radius)
00523 {
00524
00525 if (!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_))
00526 {
00527 ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius");
00528 return false;
00529 }
00530 }
00531
00532 return true;
00533 }
00534
00535 void DiffDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
00536 {
00537
00538 XmlRpc::XmlRpcValue pose_cov_list;
00539 controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
00540 ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00541 ROS_ASSERT(pose_cov_list.size() == 6);
00542 for (int i = 0; i < pose_cov_list.size(); ++i)
00543 ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00544
00545 XmlRpc::XmlRpcValue twist_cov_list;
00546 controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
00547 ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00548 ROS_ASSERT(twist_cov_list.size() == 6);
00549 for (int i = 0; i < twist_cov_list.size(); ++i)
00550 ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00551
00552
00553 odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
00554 odom_pub_->msg_.header.frame_id = odom_frame_id_;
00555 odom_pub_->msg_.child_frame_id = base_frame_id_;
00556 odom_pub_->msg_.pose.pose.position.z = 0;
00557 odom_pub_->msg_.pose.covariance = boost::assign::list_of
00558 (static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
00559 (0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
00560 (0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
00561 (0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
00562 (0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
00563 (0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
00564 odom_pub_->msg_.twist.twist.linear.y = 0;
00565 odom_pub_->msg_.twist.twist.linear.z = 0;
00566 odom_pub_->msg_.twist.twist.angular.x = 0;
00567 odom_pub_->msg_.twist.twist.angular.y = 0;
00568 odom_pub_->msg_.twist.covariance = boost::assign::list_of
00569 (static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
00570 (0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
00571 (0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
00572 (0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
00573 (0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
00574 (0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
00575 tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
00576 tf_odom_pub_->msg_.transforms.resize(1);
00577 tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
00578 tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
00579 tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
00580 }
00581
00582 }
00583