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 >= cfg_trans_tolerance_ || diff_y >= cfg_trans_tolerance_
117 || diff_yaw >= cfg_ori_tolerance_) {
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");
233 if (connected_history_) {
235 ac_ =
new MoveBaseClient(
"move_base",
false);
236 connected_history_ =
false;
240 connected_history_ =
true;
245 if (msg->msgid() == 0 || msg->msgid() == nav_if_->
msgid()) {
257 "Received stop message for non-active command " 258 "(got %u, running %u)",
267 "Cartesian goto message received (x,y,ori) = (%f,%f,%f)",
270 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0);
280 goal_position_x = nav_if_->
dest_x();
281 goal_position_y = nav_if_->
dest_y();
282 goal_position_yaw = nav_if_->
dest_ori();
286 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->
target_frame()) != 0) {
287 transform_to_fixed_frame();
297 "Cartesian goto message received (x,y,ori) = (%f,%f,%f)",
300 std::isfinite(msg->orientation()) ? msg->orientation() : 0.0);
310 goal_position_x = nav_if_->
dest_x();
311 goal_position_y = nav_if_->
dest_y();
312 goal_position_yaw = nav_if_->
dest_ori();
316 if (strcmp(cfg_fixed_frame_.c_str(), nav_if_->
target_frame()) != 0) {
317 transform_to_fixed_frame();
326 "Polar goto message received (phi,dist) = (%f,%f)",
329 nav_if_->
set_dest_x(msg->dist() * cos(msg->phi()));
330 nav_if_->
set_dest_y(msg->dist() * cos(msg->phi()));
345 set_dynreconf_value(cfg_dynreconf_trans_vel_name_, msg->max_velocity());
357 set_dynreconf_value(cfg_dynreconf_rot_vel_name_, msg->max_rotation());
380 RosNavigatorThread::load_config()
384 cfg_dynreconf_trans_vel_name_ =
config->
get_string(cfg_prefix_ +
"/dynreconf/trans_vel_name");
385 cfg_dynreconf_rot_vel_name_ =
config->
get_string(cfg_prefix_ +
"/dynreconf/rot_vel_name");
387 cfg_ori_tolerance_ =
config->
get_float(cfg_prefix_ +
"/ori_tolerance");
388 cfg_trans_tolerance_ =
config->
get_float(cfg_prefix_ +
"/trans_tolerance");
398 RosNavigatorThread::transform_to_fixed_frame()
400 fawkes::tf::Quaternion goal_q = fawkes::tf::create_quaternion_from_yaw(nav_if_->
dest_ori());
401 fawkes::tf::Point goal_p(nav_if_->
dest_x(), nav_if_->
dest_y(), 0.);
402 fawkes::tf::Pose goal_pos(goal_q, goal_p);
413 goal_position_x = goal_pos_stamped_transformed.getOrigin().getX();
414 goal_position_y = goal_pos_stamped_transformed.getOrigin().getY();
415 goal_position_yaw = fawkes::tf::get_yaw(goal_pos_stamped_transformed.getRotation());
void set_target_frame(const char *new_target_frame)
Set target_frame value.
virtual void finalize()
Finalize the thread.
bool msgq_empty()
Check if queue is empty.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Fawkes library namespace.
void set_final(const bool new_final)
Set final value.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
A class for handling time.
PolarGotoMessage Fawkes BlackBoard Interface Message.
virtual const char * what() const
Get primary string.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
float dest_ori() const
Get dest_ori value.
Logger * logger
This is the Logger member used to access the logger.
void set_security_distance(const float new_security_distance)
Set security_distance value.
Clock * clock
By means of this member access to the clock is given.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
float dest_y() const
Get dest_y value.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
virtual void init()
Initialize the thread.
Base class for exceptions in Fawkes.
SetMaxRotationMessage Fawkes BlackBoard Interface Message.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
CartesianGotoWithFrameMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
const char * name() const
Get name of thread.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
char * target_frame() const
Get target_frame value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
float dest_x() const
Get dest_x value.
void msgq_flush()
Flush all messages.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Time & stamp()
Set this time to the current time.
void set_dest_y(const float new_dest_y)
Set dest_y value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
uint32_t msgid() const
Get msgid value.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
void append(const char *format,...)
Append messages to the message list.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
RosNavigatorThread(std::string &cfg_prefix)
Constructor.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
virtual void close(Interface *interface)=0
Close interface.