22 #include "navigator_thread.h"
24 using namespace fawkes;
33 :
Thread(
"RosNavigatorThread",
Thread::OPMODE_WAITFORWAKEUP),
45 e.
append(
"%s initialization failed, could not open navigator "
46 "interface for writing",
name());
52 ac_ =
new MoveBaseClient(
"move_base",
false);
56 connected_history_ =
false;
73 RosNavigatorThread::check_status()
76 if(ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
80 else if(ac_->getState() == actionlib::SimpleClientGoalState::ABORTED ||
81 ac_->getState() == actionlib::SimpleClientGoalState::REJECTED){
94 RosNavigatorThread::send_goal()
97 goal_.target_pose.header.frame_id =
"base_link";
98 goal_.target_pose.header.stamp = ros::Time::now();
99 goal_.target_pose.pose.position.x = nav_if_->
dest_x();
100 goal_.target_pose.pose.position.y = nav_if_->
dest_y();
101 fawkes::tf::Quaternion q(nav_if_->
dest_ori(), 0, 0);
102 goal_.target_pose.pose.orientation.x = q.x();
103 goal_.target_pose.pose.orientation.y = q.y();
104 goal_.target_pose.pose.orientation.z = q.z();
105 goal_.target_pose.pose.orientation.w = q.w();
107 ac_->sendGoal(goal_);
115 if(ac_->isServerConnected()){
117 connected_history_ =
true;
145 msg->phi(), msg->dist());
146 nav_if_->
set_dest_x(msg->dist() * cos(msg->phi()));
147 nav_if_->
set_dest_y(msg->dist() * cos(msg->phi()));
180 if (connected_history_){
182 ac_ =
new MoveBaseClient(
"move_base",
false);
183 connected_history_ =
false;