00001 #include <cmath>
00002
00003 #include <tf/transform_datatypes.h>
00004
00005 #include <boost/assign.hpp>
00006
00007 #include <four_wheel_steering_controller/four_wheel_steering_controller.h>
00008 #include <urdf_vehicle_kinematic/urdf_vehicle_kinematic.h>
00009
00010 namespace four_wheel_steering_controller{
00011
00012 FourWheelSteeringController::FourWheelSteeringController()
00013 : open_loop_(false)
00014 , command_struct_()
00015 , command_struct_four_wheel_steering_()
00016 , track_(0.0)
00017 , wheel_radius_(0.0)
00018 , wheel_base_(0.0)
00019 , cmd_vel_timeout_(0.5)
00020 , base_frame_id_("base_link")
00021 , enable_odom_tf_(true)
00022 , enable_twist_cmd_(false)
00023 {
00024 }
00025
00026 bool FourWheelSteeringController::initRequest(hardware_interface::RobotHW *const robot_hw,
00027 ros::NodeHandle& root_nh,
00028 ros::NodeHandle& ctrlr_nh,
00029 std::set<std::string> &claimed_resources)
00030 {
00031 if (state_ != CONSTRUCTED)
00032 {
00033 ROS_ERROR("The four_wheel_steering controller could not be created.");
00034 return false;
00035 }
00036
00037 hardware_interface::PositionJointInterface *const pos_joint_hw = robot_hw->get<hardware_interface::PositionJointInterface>();
00038 hardware_interface::VelocityJointInterface *const vel_joint_hw = robot_hw->get<hardware_interface::VelocityJointInterface>();
00039
00040 if (pos_joint_hw == NULL)
00041 {
00042 ROS_ERROR("This controller requires a hardware interface of type '%s'."
00043 " Make sure this is registered in the hardware_interface::RobotHW class.",
00044 hardware_interface::internal::demangledTypeName<hardware_interface::PositionJointInterface>().c_str());
00045 return false;
00046 }
00047 else if (vel_joint_hw == NULL)
00048 {
00049 ROS_ERROR("This controller requires a hardware interface of type '%s'."
00050 " Make sure this is registered in the hardware_interface::RobotHW class.",
00051 hardware_interface::internal::demangledTypeName<hardware_interface::PositionJointInterface>().c_str());
00052 return false;
00053 }
00054
00055 pos_joint_hw->clearClaims();
00056 vel_joint_hw->clearClaims();
00057 if(init(pos_joint_hw, vel_joint_hw, root_nh, ctrlr_nh) == false)
00058 {
00059 ROS_ERROR("Failed to initialize the controller");
00060 return false;
00061 }
00062
00063 claimed_resources.clear();
00064 const std::set<std::string> claims_pos = pos_joint_hw->getClaims();
00065 claimed_resources.insert(claims_pos.begin(), claims_pos.end());
00066 pos_joint_hw->clearClaims();
00067
00068 const std::set<std::string> claims_vel = vel_joint_hw->getClaims();
00069 claimed_resources.insert(claims_vel.begin(), claims_vel.end());
00070 vel_joint_hw->clearClaims();
00071
00072 state_ = INITIALIZED;
00073 return true;
00074 }
00075
00076 bool FourWheelSteeringController::init(hardware_interface::PositionJointInterface* hw_pos,
00077 hardware_interface::VelocityJointInterface* hw_vel,
00078 ros::NodeHandle& root_nh,
00079 ros::NodeHandle &controller_nh)
00080 {
00081 const std::string complete_ns = controller_nh.getNamespace();
00082 std::size_t id = complete_ns.find_last_of("/");
00083 name_ = complete_ns.substr(id + 1);
00084
00085
00086 std::vector<std::string> front_wheel_names, rear_wheel_names;
00087 if (!getWheelNames(controller_nh, "front_wheel", front_wheel_names) ||
00088 !getWheelNames(controller_nh, "rear_wheel", rear_wheel_names))
00089 {
00090 return false;
00091 }
00092
00093 if (front_wheel_names.size() != rear_wheel_names.size())
00094 {
00095 ROS_ERROR_STREAM_NAMED(name_,
00096 "#front wheels (" << front_wheel_names.size() << ") != " <<
00097 "#rear wheels (" << rear_wheel_names.size() << ").");
00098 return false;
00099 }
00100 else if (front_wheel_names.size() != 2)
00101 {
00102 ROS_ERROR_STREAM_NAMED(name_,
00103 "#two wheels by axle (left and right) is needed; now : "<<front_wheel_names.size()<<" .");
00104 return false;
00105 }
00106 else
00107 {
00108 front_wheel_joints_.resize(front_wheel_names.size());
00109 rear_wheel_joints_.resize(front_wheel_names.size());
00110 }
00111
00112
00113 std::vector<std::string> front_steering_names, rear_steering_names;
00114 if (!getWheelNames(controller_nh, "front_steering", front_steering_names) ||
00115 !getWheelNames(controller_nh, "rear_steering", rear_steering_names))
00116 {
00117 return false;
00118 }
00119
00120 if (front_steering_names.size() != rear_steering_names.size())
00121 {
00122 ROS_ERROR_STREAM_NAMED(name_,
00123 "#left steerings (" << front_steering_names.size() << ") != " <<
00124 "#right steerings (" << rear_steering_names.size() << ").");
00125 return false;
00126 }
00127 else if (front_steering_names.size() != 2)
00128 {
00129 ROS_ERROR_STREAM_NAMED(name_,
00130 "#two steering by axle (left and right) is needed; now : "<<front_steering_names.size()<<" .");
00131 return false;
00132 }
00133 else
00134 {
00135 front_steering_joints_.resize(front_steering_names.size());
00136 rear_steering_joints_.resize(front_steering_names.size());
00137 }
00138
00139
00140 double publish_rate;
00141 controller_nh.param("publish_rate", publish_rate, 50.0);
00142 ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
00143 << publish_rate << "Hz.");
00144 publish_period_ = ros::Duration(1.0 / publish_rate);
00145
00146 controller_nh.param("open_loop", open_loop_, open_loop_);
00147
00148 int velocity_rolling_window_size = 10;
00149 controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
00150 ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of "
00151 << velocity_rolling_window_size << ".");
00152
00153 odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
00154
00155
00156 controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
00157 ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
00158 << cmd_vel_timeout_ << "s.");
00159
00160 controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
00161 ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
00162
00163 controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
00164 ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));
00165
00166 controller_nh.param("enable_twist_cmd", enable_twist_cmd_, enable_twist_cmd_);
00167 ROS_INFO_STREAM_NAMED(name_, "Twist cmd is " << (enable_twist_cmd_?"enabled":"disabled")<<" (default is four_wheel_steering)");
00168
00169
00170 controller_nh.param("linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits );
00171 controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
00172 controller_nh.param("linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity );
00173 controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity );
00174 controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration );
00175 controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration );
00176
00177 controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits );
00178 controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
00179 controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity );
00180 controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
00181 controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
00182 controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
00183
00184
00185 bool lookup_track = !controller_nh.getParam("track", track_);
00186 bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
00187 bool lookup_wheel_base = !controller_nh.getParam("wheel_base", wheel_base_);
00188
00189 urdf_vehicle_kinematic::UrdfVehicleKinematic uvk(root_nh, base_frame_id_);
00190 if(lookup_track)
00191 if(!uvk.getDistanceBetweenJoints(front_steering_names[0], front_steering_names[1], track_))
00192 return false;
00193 else
00194 controller_nh.setParam("track",track_);
00195 if(lookup_wheel_radius)
00196 if(!uvk.getJointRadius(front_wheel_names[0], wheel_radius_))
00197 return false;
00198 else
00199 controller_nh.setParam("wheel_radius",wheel_radius_);
00200 if(lookup_wheel_base)
00201 if(!uvk.getDistanceBetweenJoints(front_wheel_names[0], rear_wheel_names[0], wheel_base_))
00202 return false;
00203 else
00204 controller_nh.setParam("wheel_base",wheel_base_);
00205
00206
00207
00208 const double ws = track_;
00209 const double wr = wheel_radius_;
00210 const double wb = wheel_base_;
00211 odometry_.setWheelParams(ws, wr, wb);
00212 ROS_INFO_STREAM_NAMED(name_,
00213 "Odometry params : wheel separation " << ws
00214 << ", wheel radius " << wr
00215 << ", wheel base " << wb);
00216
00217 setOdomPubFields(root_nh, controller_nh);
00218
00219
00220 for (int i = 0; i < front_wheel_joints_.size(); ++i)
00221 {
00222 ROS_INFO_STREAM_NAMED(name_,
00223 "Adding left wheel with joint name: " << front_wheel_names[i]
00224 << " and right wheel with joint name: " << rear_wheel_names[i]);
00225 front_wheel_joints_[i] = hw_vel->getHandle(front_wheel_names[i]);
00226 rear_wheel_joints_[i] = hw_vel->getHandle(rear_wheel_names[i]);
00227 }
00228
00229
00230 for (int i = 0; i < front_steering_joints_.size(); ++i)
00231 {
00232 ROS_INFO_STREAM_NAMED(name_,
00233 "Adding left steering with joint name: " << front_steering_names[i]
00234 << " and right steering with joint name: " << rear_steering_names[i]);
00235 front_steering_joints_[i] = hw_pos->getHandle(front_steering_names[i]);
00236 rear_steering_joints_[i] = hw_pos->getHandle(rear_steering_names[i]);
00237 }
00238
00239 if(enable_twist_cmd_ == true)
00240 sub_command_ = controller_nh.subscribe("cmd_vel", 1, &FourWheelSteeringController::cmdVelCallback, this);
00241 else
00242 sub_command_four_wheel_steering_ = controller_nh.subscribe("cmd_four_wheel_steering", 1, &FourWheelSteeringController::cmdFourWheelSteeringCallback, this);
00243
00244 return true;
00245 }
00246
00247 void FourWheelSteeringController::update(const ros::Time& time, const ros::Duration& period)
00248 {
00249
00250 if (open_loop_)
00251 {
00252 odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
00253 }
00254 else
00255 {
00256 const double fl_speed = front_wheel_joints_[0].getVelocity();
00257 const double fr_speed = front_wheel_joints_[1].getVelocity();
00258 const double rl_speed = rear_wheel_joints_[0].getVelocity();
00259 const double rr_speed = rear_wheel_joints_[1].getVelocity();
00260 if (std::isnan(fl_speed) || std::isnan(fr_speed)
00261 || std::isnan(rl_speed) || std::isnan(rr_speed))
00262 return;
00263
00264 const double fl_steering = front_steering_joints_[0].getPosition();
00265 const double fr_steering = front_steering_joints_[1].getPosition();
00266 const double rl_steering = rear_steering_joints_[0].getPosition();
00267 const double rr_steering = rear_steering_joints_[1].getPosition();
00268 if (std::isnan(fl_steering) || std::isnan(fr_steering)
00269 || std::isnan(rl_steering) || std::isnan(rr_steering))
00270 return;
00271 double front_steering_pos = 0.0;
00272 if(fabs(fl_steering) > 0.001 || fabs(fr_steering) > 0.001)
00273 {
00274 front_steering_pos = atan(2*tan(fl_steering)*tan(fr_steering)/
00275 (tan(fl_steering) + tan(fr_steering)));
00276 }
00277 double rear_steering_pos = 0.0;
00278 if(fabs(rl_steering) > 0.001 || fabs(rr_steering) > 0.001)
00279 {
00280 rear_steering_pos = atan(2*tan(rl_steering)*tan(rr_steering)/
00281 (tan(rl_steering) + tan(rr_steering)));
00282 }
00283
00284 ROS_DEBUG_STREAM_THROTTLE(1, "rl_steering "<<rl_steering<<" rr_steering "<<rr_steering<<" rear_steering_pos "<<rear_steering_pos);
00285
00286 odometry_.update(fl_speed, fr_speed, rl_speed, rr_speed,
00287 front_steering_pos, rear_steering_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_.getLinearX();
00306 odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY();
00307 odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
00308 odom_pub_->unlockAndPublish();
00309 }
00310
00311
00312 if (enable_odom_tf_ && tf_odom_pub_->trylock())
00313 {
00314 geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
00315 odom_frame.header.stamp = time;
00316 odom_frame.transform.translation.x = odometry_.getX();
00317 odom_frame.transform.translation.y = odometry_.getY();
00318 odom_frame.transform.rotation = orientation;
00319 tf_odom_pub_->unlockAndPublish();
00320 }
00321 }
00322
00323
00324
00325 Commands curr_cmd;
00326 if(enable_twist_cmd_ == false)
00327 curr_cmd = *(command_four_wheel_steering_.readFromRT());
00328 else
00329 curr_cmd = *(command_.readFromRT());
00330
00331 const double dt = (time - curr_cmd.stamp).toSec();
00332
00333
00334 if (dt > cmd_vel_timeout_)
00335 {
00336 curr_cmd.lin = 0.0;
00337 curr_cmd.ang = 0.0;
00338 curr_cmd.front_steering = 0.0;
00339 curr_cmd.rear_steering = 0.0;
00340 }
00341
00342
00343 const double cmd_dt(period.toSec());
00344
00345 limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
00346 limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
00347
00348 last1_cmd_ = last0_cmd_;
00349 last0_cmd_ = curr_cmd;
00350
00351
00352 const double angular_speed = odometry_.getAngular();
00353
00354 ROS_DEBUG_STREAM("angular_speed "<<angular_speed<<" curr_cmd.lin "<<curr_cmd.lin<< " wheel_radius_ "<<wheel_radius_);
00355 double vel_left_front = 0, vel_right_front = 0;
00356 double vel_left_rear = 0, vel_right_rear = 0;
00357 double front_left_steering = 0, front_right_steering = 0;
00358 double rear_left_steering = 0, rear_right_steering = 0;
00359 if(enable_twist_cmd_ == true)
00360 {
00361
00362 if(fabs(curr_cmd.lin) > 0.001)
00363 {
00364 vel_left_front = copysign(1.0, curr_cmd.lin) * sqrt((pow(curr_cmd.lin - curr_cmd.ang*track_/2,2)
00365 +pow(wheel_base_*curr_cmd.ang/2.0,2)))/wheel_radius_;
00366 vel_right_front = copysign(1.0, curr_cmd.lin) * sqrt((pow(curr_cmd.lin + curr_cmd.ang*track_/2,2)
00367 +pow(wheel_base_*curr_cmd.ang/2.0,2)))/wheel_radius_;
00368 vel_left_rear = copysign(1.0, curr_cmd.lin) * sqrt((pow(curr_cmd.lin - curr_cmd.ang*track_/2,2)
00369 +pow(wheel_base_*curr_cmd.ang/2.0,2)))/wheel_radius_;
00370 vel_right_rear = copysign(1.0, curr_cmd.lin) * sqrt((pow(curr_cmd.lin + curr_cmd.ang*track_/2,2)
00371 +pow(wheel_base_*curr_cmd.ang/2.0,2)))/wheel_radius_;
00372 }
00373
00374
00375 if(fabs(2.0*curr_cmd.lin) > fabs(curr_cmd.ang*track_))
00376 {
00377 front_left_steering = atan(curr_cmd.ang*wheel_base_ /
00378 (2.0*curr_cmd.lin - curr_cmd.ang*track_));
00379 front_right_steering = atan(curr_cmd.ang*wheel_base_ /
00380 (2.0*curr_cmd.lin + curr_cmd.ang*track_));
00381 rear_left_steering = -atan(curr_cmd.ang*wheel_base_ /
00382 (2.0*curr_cmd.lin - curr_cmd.ang*track_));
00383 rear_right_steering = -atan(curr_cmd.ang*wheel_base_ /
00384 (2.0*curr_cmd.lin + curr_cmd.ang*track_));
00385 }
00386 else if(fabs(curr_cmd.lin) > 0.001)
00387 {
00388 front_left_steering = copysign(M_PI_2, curr_cmd.ang);
00389 front_right_steering = copysign(M_PI_2, curr_cmd.ang);
00390 rear_left_steering = copysign(M_PI_2, -curr_cmd.ang);
00391 rear_right_steering = copysign(M_PI_2, -curr_cmd.ang);
00392 }
00393
00394 }
00395 else
00396 {
00397
00398 double steering_diff = track_*(tan(curr_cmd.front_steering) - tan(curr_cmd.rear_steering))/2.0;
00399 if(fabs(wheel_base_ - fabs(steering_diff)) > 0.001)
00400 {
00401 front_left_steering = wheel_base_*curr_cmd.front_steering/(wheel_base_-steering_diff);
00402 front_right_steering = wheel_base_*curr_cmd.front_steering/(wheel_base_+steering_diff);
00403 rear_left_steering = wheel_base_*curr_cmd.rear_steering/(wheel_base_-steering_diff);
00404 rear_right_steering = wheel_base_*curr_cmd.rear_steering/(wheel_base_+steering_diff);
00405 }
00406
00407
00408 if(fabs(curr_cmd.lin) > 0.001)
00409 {
00410
00411
00412 double l_front = 0;
00413 if(fabs(tan(front_left_steering) - tan(front_right_steering)) > 0.01)
00414 {
00415 l_front = tan(front_right_steering) * tan(front_left_steering) * track_
00416 / (tan(front_left_steering) - tan(front_right_steering));
00417 }
00418
00419 double l_rear = 0;
00420 if(fabs(tan(rear_left_steering) - tan(rear_right_steering)) > 0.01)
00421 {
00422 l_rear = tan(rear_right_steering) * tan(rear_left_steering) * track_
00423 / (tan(rear_left_steering) - tan(rear_right_steering));
00424 }
00425
00426 double angular_speed_cmd = curr_cmd.lin * (tan(curr_cmd.front_steering)-tan(curr_cmd.rear_steering))/wheel_base_;
00427
00428 vel_left_front = copysign(1.0, curr_cmd.lin) * sqrt((pow(curr_cmd.lin - angular_speed_cmd*track_/2,2)
00429 +pow(l_front*angular_speed_cmd,2)))/wheel_radius_;
00430 vel_right_front = copysign(1.0, curr_cmd.lin) * sqrt((pow(curr_cmd.lin + angular_speed_cmd*track_/2,2)
00431 +pow(wheel_base_*angular_speed_cmd/2.0,2)))/wheel_radius_;
00432 vel_left_rear = copysign(1.0, curr_cmd.lin) * sqrt((pow(curr_cmd.lin - angular_speed_cmd*track_/2,2)
00433 +pow(wheel_base_*angular_speed_cmd/2.0,2)))/wheel_radius_;
00434 vel_right_rear = copysign(1.0, curr_cmd.lin) * sqrt((pow(curr_cmd.lin + angular_speed_cmd*track_/2,2)
00435 +pow(wheel_base_*angular_speed_cmd/2.0,2)))/wheel_radius_;
00436 }
00437
00438 }
00439
00440 ROS_DEBUG_STREAM_THROTTLE(1, "vel_left_rear "<<vel_left_rear<<" front_right_steering "<<front_right_steering);
00441
00442 if(front_wheel_joints_.size() == 2 && rear_wheel_joints_.size() == 2)
00443 {
00444 front_wheel_joints_[0].setCommand(vel_left_front);
00445 front_wheel_joints_[1].setCommand(vel_right_front);
00446 rear_wheel_joints_[0].setCommand(vel_left_rear);
00447 rear_wheel_joints_[1].setCommand(vel_right_rear);
00448 }
00449
00451 if(front_steering_joints_.size() == 2 && rear_steering_joints_.size() == 2)
00452 {
00453 ROS_DEBUG_STREAM("front_left_steering "<<front_left_steering<<" rear_right_steering "<<rear_right_steering);
00454 front_steering_joints_[0].setCommand(front_left_steering);
00455 front_steering_joints_[1].setCommand(front_right_steering);
00456 rear_steering_joints_[0].setCommand(rear_left_steering);
00457 rear_steering_joints_[1].setCommand(rear_right_steering);
00458 }
00459 }
00460
00461 void FourWheelSteeringController::starting(const ros::Time& time)
00462 {
00463 brake();
00464
00465
00466 last_state_publish_time_ = time;
00467
00468 odometry_.init(time);
00469 }
00470
00471 void FourWheelSteeringController::stopping(const ros::Time& )
00472 {
00473 brake();
00474 }
00475
00476 void FourWheelSteeringController::brake()
00477 {
00478 const double vel = 0.0;
00479 for (size_t i = 0; i < front_wheel_joints_.size(); ++i)
00480 {
00481 front_wheel_joints_[i].setCommand(vel);
00482 rear_wheel_joints_[i].setCommand(vel);
00483 }
00484
00485 const double pos = 0.0;
00486 for (size_t i = 0; i < front_steering_joints_.size(); ++i)
00487 {
00488 front_steering_joints_[i].setCommand(pos);
00489 rear_steering_joints_[i].setCommand(pos);
00490 }
00491 }
00492
00493 void FourWheelSteeringController::cmdVelCallback(const geometry_msgs::Twist& command)
00494 {
00495 if (isRunning())
00496 {
00497 command_struct_.ang = command.angular.z;
00498 command_struct_.lin = command.linear.x;
00499 command_struct_.stamp = ros::Time::now();
00500 command_.writeFromNonRT (command_struct_);
00501 ROS_DEBUG_STREAM_NAMED(name_,
00502 "Added values to command. "
00503 << "Ang: " << command_struct_.ang << ", "
00504 << "Lin: " << command_struct_.lin << ", "
00505 << "Stamp: " << command_struct_.stamp);
00506 }
00507 else
00508 {
00509 ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
00510 }
00511 }
00512
00513 void FourWheelSteeringController::cmdFourWheelSteeringCallback(const four_wheel_steering_msgs::FourWheelSteering& command)
00514 {
00515 if (isRunning())
00516 {
00517 command_struct_four_wheel_steering_.front_steering = command.front_steering_angle;
00518 command_struct_four_wheel_steering_.rear_steering = command.rear_steering_angle;
00519 command_struct_four_wheel_steering_.lin = command.speed;
00520 command_struct_four_wheel_steering_.stamp = ros::Time::now();
00521 command_four_wheel_steering_.writeFromNonRT (command_struct_four_wheel_steering_);
00522 ROS_DEBUG_STREAM_NAMED(name_,
00523 "Added values to command. "
00524 << "Steering front : " << command_struct_four_wheel_steering_.front_steering << ", "
00525 << "Steering rear : " << command_struct_four_wheel_steering_.rear_steering << ", "
00526 << "Lin: " << command_struct_four_wheel_steering_.lin << ", "
00527 << "Stamp: " << command_struct_four_wheel_steering_.stamp);
00528 }
00529 else
00530 {
00531 ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
00532 }
00533 }
00534
00535 bool FourWheelSteeringController::getWheelNames(ros::NodeHandle& controller_nh,
00536 const std::string& wheel_param,
00537 std::vector<std::string>& wheel_names)
00538 {
00539 XmlRpc::XmlRpcValue wheel_list;
00540 if (!controller_nh.getParam(wheel_param, wheel_list))
00541 {
00542 ROS_ERROR_STREAM_NAMED(name_,
00543 "Couldn't retrieve wheel param '" << wheel_param << "'.");
00544 return false;
00545 }
00546
00547 if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
00548 {
00549 if (wheel_list.size() == 0)
00550 {
00551 ROS_ERROR_STREAM_NAMED(name_,
00552 "Wheel param '" << wheel_param << "' is an empty list");
00553 return false;
00554 }
00555
00556 for (int i = 0; i < wheel_list.size(); ++i)
00557 {
00558 if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00559 {
00560 ROS_ERROR_STREAM_NAMED(name_,
00561 "Wheel param '" << wheel_param << "' #" << i <<
00562 " isn't a string.");
00563 return false;
00564 }
00565 }
00566
00567 wheel_names.resize(wheel_list.size());
00568 for (int i = 0; i < wheel_list.size(); ++i)
00569 {
00570 wheel_names[i] = static_cast<std::string>(wheel_list[i]);
00571
00572 }
00573 }
00574 else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
00575 {
00576 wheel_names.push_back(wheel_list);
00577 }
00578 else
00579 {
00580 ROS_ERROR_STREAM_NAMED(name_,
00581 "Wheel param '" << wheel_param <<
00582 "' is neither a list of strings nor a string.");
00583 return false;
00584 }
00585 return true;
00586 }
00587
00588 void FourWheelSteeringController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
00589 {
00590
00591 XmlRpc::XmlRpcValue pose_cov_list;
00592 controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
00593 ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00594 ROS_ASSERT(pose_cov_list.size() == 6);
00595 for (int i = 0; i < pose_cov_list.size(); ++i)
00596 ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00597
00598 XmlRpc::XmlRpcValue twist_cov_list;
00599 controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
00600 ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00601 ROS_ASSERT(twist_cov_list.size() == 6);
00602 for (int i = 0; i < twist_cov_list.size(); ++i)
00603 ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00604
00605
00606 odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
00607 odom_pub_->msg_.header.frame_id = "odom";
00608 odom_pub_->msg_.child_frame_id = base_frame_id_;
00609 odom_pub_->msg_.pose.pose.position.z = 0;
00610 odom_pub_->msg_.pose.covariance = boost::assign::list_of
00611 (static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
00612 (0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
00613 (0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
00614 (0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
00615 (0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
00616 (0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
00617 odom_pub_->msg_.twist.twist.linear.y = 0;
00618 odom_pub_->msg_.twist.twist.linear.z = 0;
00619 odom_pub_->msg_.twist.twist.angular.x = 0;
00620 odom_pub_->msg_.twist.twist.angular.y = 0;
00621 odom_pub_->msg_.twist.covariance = boost::assign::list_of
00622 (static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
00623 (0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
00624 (0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
00625 (0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
00626 (0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
00627 (0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
00628 tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
00629 tf_odom_pub_->msg_.transforms.resize(1);
00630 tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
00631 tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
00632 tf_odom_pub_->msg_.transforms[0].header.frame_id = "odom";
00633 }
00634
00635 }