22 #include "navigator_thread.h"
35 :
Thread(
"RosNavigatorThread",
Thread::OPMODE_WAITFORWAKEUP),
37 cfg_prefix_(cfg_prefix)
48 e.
append(
"%s initialization failed, could not open navigator "
49 "interface for writing",
56 ac_ =
new MoveBaseClient(
"move_base",
false);
59 connected_history_ =
false;
78 delete ac_init_checktime_;
82 RosNavigatorThread::check_status()
85 if (
rosnode->hasParam(cfg_dynreconf_path_ +
"/" + cfg_dynreconf_trans_vel_name_)) {
86 rosnode->getParam(cfg_dynreconf_path_ +
"/" + cfg_dynreconf_trans_vel_name_, param_max_vel);
90 if (
rosnode->hasParam(cfg_dynreconf_path_ +
"/" + cfg_dynreconf_rot_vel_name_)) {
91 rosnode->getParam(cfg_dynreconf_path_ +
"/" + cfg_dynreconf_rot_vel_name_, param_max_rot);
97 if (ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED
98 || ac_->getState() == actionlib::SimpleClientGoalState::PREEMPTED) {
102 fawkes::tf::Quaternion q_base_rotation;
103 q_base_rotation.setX(base_position.pose.orientation.x);
104 q_base_rotation.setY(base_position.pose.orientation.y);
105 q_base_rotation.setZ(base_position.pose.orientation.z);
106 q_base_rotation.setW(base_position.pose.orientation.w);
108 double base_position_x = base_position.pose.position.x;
109 double base_position_y = base_position.pose.position.y;
110 double base_position_yaw = fawkes::tf::get_yaw(q_base_rotation);
112 double diff_x = fabs(base_position_x - goal_position_x);
113 double diff_y = fabs(base_position_y - goal_position_y);
116 if (diff_x >= goal_tolerance_trans || diff_y >= goal_tolerance_trans
117 || diff_yaw >= goal_tolerance_yaw) {
123 }
else if (ac_->getState() == actionlib::SimpleClientGoalState::LOST) {
126 }
else if (ac_->getState() == actionlib::SimpleClientGoalState::ABORTED
127 || ac_->getState() == actionlib::SimpleClientGoalState::REJECTED) {
141 RosNavigatorThread::send_goal()
144 goal_.target_pose.header.frame_id = nav_if_->
target_frame();
145 goal_.target_pose.header.stamp = ros::Time::now();
146 goal_.target_pose.pose.position.x = nav_if_->
dest_x();
147 goal_.target_pose.pose.position.y = nav_if_->
dest_y();
149 fawkes::tf::Quaternion q(std::isfinite(nav_if_->
dest_ori()) ? nav_if_->
dest_ori() : 0.0, 0, 0);
150 goal_.target_pose.pose.orientation.x = q.x();
151 goal_.target_pose.pose.orientation.y = q.y();
152 goal_.target_pose.pose.orientation.z = q.z();
153 goal_.target_pose.pose.orientation.w = q.w();
156 boost::bind(&RosNavigatorThread::doneCb,
this, _1, _2),
157 boost::bind(&RosNavigatorThread::activeCb,
this),
158 boost::bind(&RosNavigatorThread::feedbackCb,
this, _1));
165 RosNavigatorThread::activeCb()
171 RosNavigatorThread::feedbackCb(
const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)
173 base_position = feedback->base_position;
177 RosNavigatorThread::doneCb(
const actionlib::SimpleClientGoalState & state,
178 const move_base_msgs::MoveBaseResultConstPtr &result)
184 RosNavigatorThread::set_dynreconf_value(
const std::string &path,
const float value)
186 dynreconf_double_param.name = path;
187 dynreconf_double_param.value = value;
188 dynreconf_conf.doubles.push_back(dynreconf_double_param);
189 dynreconf_srv_req.config = dynreconf_conf;
191 if (!ros::service::call(cfg_dynreconf_path_ +
"/set_parameters",
193 dynreconf_srv_resp)) {
195 "Error in setting dynreconf value %s to %f in path %s",
198 cfg_dynreconf_path_.c_str());
199 dynreconf_conf.doubles.clear();
203 dynreconf_conf.doubles.clear();
209 RosNavigatorThread::stop_goals()
212 ac_->cancelAllGoals();
218 if (!ac_->isServerConnected()) {
220 if (now - ac_init_checktime_ >= 5.0) {
223 ac_ =
new MoveBaseClient(
"move_base",
false);
224 ac_init_checktime_->
stamp();
228 "Command received while ROS ActionClient "
229 "not reachable, ignoring");
235 if (connected_history_) {
237 ac_ =
new MoveBaseClient(
"move_base",
false);
238 connected_history_ =
false;
242 connected_history_ =
true;
247 if (msg->msgid() == 0 || msg->msgid() == nav_if_->
msgid()) {
259 "Received stop message for non-active command "
260 "(got %u, running %u)",
269 "Cartesian goto message received (x,y,ori) = (%f,%f,%f)",
272 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0);
282 goal_position_x = nav_if_->
dest_x();
283 goal_position_y = nav_if_->
dest_y();
284 goal_position_yaw = nav_if_->
dest_ori();
286 goal_tolerance_trans = cfg_trans_tolerance_;
287 goal_tolerance_yaw = cfg_ori_tolerance_;
291 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->
target_frame()) != 0) {
292 transform_to_fixed_frame();
302 "Cartesian goto message received (x,y,ori) = (%f,%f,%f)",
305 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0);
315 goal_position_x = nav_if_->
dest_x();
316 goal_position_y = nav_if_->
dest_y();
317 goal_position_yaw = nav_if_->
dest_ori();
319 goal_tolerance_trans = cfg_trans_tolerance_;
320 goal_tolerance_yaw = cfg_ori_tolerance_;
324 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->
target_frame()) != 0) {
325 transform_to_fixed_frame();
335 "Cartesian goto with tolerance message received "
336 "(x,y,ori,trans_tolerance,ori_tolerance) = (%f,%f,%f,%f,%f)",
339 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0,
340 msg->translation_tolerance(),
341 msg->orientation_tolerance());
351 goal_position_x = nav_if_->
dest_x();
352 goal_position_y = nav_if_->
dest_y();
353 goal_position_yaw = nav_if_->
dest_ori();
355 goal_tolerance_trans = msg->translation_tolerance();
356 goal_tolerance_yaw = msg->orientation_tolerance();
360 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->
target_frame()) != 0) {
361 transform_to_fixed_frame();
371 "Cartesian goto with tolerance message received "
372 "(x,y,ori,trans_tolerance,ori_tolerance) = (%f,%f,%f,%f,%f)",
375 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0,
376 msg->translation_tolerance(),
377 msg->orientation_tolerance());
387 goal_position_x = nav_if_->
dest_x();
388 goal_position_y = nav_if_->
dest_y();
389 goal_position_yaw = nav_if_->
dest_ori();
391 goal_tolerance_trans = msg->translation_tolerance();
392 goal_tolerance_yaw = msg->orientation_tolerance();
396 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->
target_frame()) != 0) {
397 transform_to_fixed_frame();
406 "Polar goto message received (phi,dist) = (%f,%f)",
409 nav_if_->
set_dest_x(msg->dist() * cos(msg->phi()));
410 nav_if_->
set_dest_y(msg->dist() * cos(msg->phi()));
415 goal_tolerance_trans = cfg_trans_tolerance_;
416 goal_tolerance_yaw = cfg_ori_tolerance_;
428 set_dynreconf_value(cfg_dynreconf_trans_vel_name_, msg->max_velocity());
440 set_dynreconf_value(cfg_dynreconf_rot_vel_name_, msg->max_rotation());
463 RosNavigatorThread::load_config()
467 cfg_dynreconf_trans_vel_name_ =
config->
get_string(cfg_prefix_ +
"/dynreconf/trans_vel_name");
468 cfg_dynreconf_rot_vel_name_ =
config->
get_string(cfg_prefix_ +
"/dynreconf/rot_vel_name");
470 cfg_ori_tolerance_ =
config->
get_float(cfg_prefix_ +
"/ori_tolerance");
471 cfg_trans_tolerance_ =
config->
get_float(cfg_prefix_ +
"/trans_tolerance");
481 RosNavigatorThread::transform_to_fixed_frame()
483 fawkes::tf::Quaternion goal_q = fawkes::tf::create_quaternion_from_yaw(nav_if_->
dest_ori());
484 fawkes::tf::Point goal_p(nav_if_->
dest_x(), nav_if_->
dest_y(), 0.);
485 fawkes::tf::Pose goal_pos(goal_q, goal_p);
496 goal_position_x = goal_pos_stamped_transformed.getOrigin().getX();
497 goal_position_y = goal_pos_stamped_transformed.getOrigin().getY();
498 goal_position_yaw = fawkes::tf::get_yaw(goal_pos_stamped_transformed.getRotation());