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_;
82RosNavigatorThread::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) {
141RosNavigatorThread::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));
165RosNavigatorThread::activeCb()
171RosNavigatorThread::feedbackCb(
const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)
173 base_position = feedback->base_position;
177RosNavigatorThread::doneCb(
const actionlib::SimpleClientGoalState & state,
178 const move_base_msgs::MoveBaseResultConstPtr &result)
184RosNavigatorThread::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();
209RosNavigatorThread::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());
463RosNavigatorThread::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");
481RosNavigatorThread::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());
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
RosNavigatorThread(std::string &cfg_prefix)
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
void append(const char *format,...) noexcept
Append messages to the message list.
void msgq_pop()
Erase first message from queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
void msgq_flush()
Flush all messages.
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
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.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
CartesianGotoWithFrameMessage Fawkes BlackBoard Interface Message.
CartesianGotoWithFrameWithToleranceMessage Fawkes BlackBoard Interface Message.
CartesianGotoWithToleranceMessage Fawkes BlackBoard Interface Message.
PolarGotoMessage Fawkes BlackBoard Interface Message.
SetMaxRotationMessage Fawkes BlackBoard Interface Message.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
SetSecurityDistanceMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
NavigatorInterface Fawkes BlackBoard Interface.
void set_max_rotation(const float new_max_rotation)
Set max_rotation value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_max_velocity(const float new_max_velocity)
Set max_velocity value.
float dest_x() const
Get dest_x value.
float dest_y() const
Get dest_y value.
char * target_frame() const
Get target_frame value.
void set_target_frame(const char *new_target_frame)
Set target_frame value.
void set_security_distance(const float new_security_distance)
Set security_distance value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
uint32_t msgid() const
Get msgid value.
void set_dest_ori(const float new_dest_ori)
Set dest_ori value.
void set_dest_y(const float new_dest_y)
Set dest_y value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
float dest_ori() const
Get dest_ori value.
void set_final(const bool new_final)
Set final value.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
A class for handling time.
Time & stamp()
Set this time to the current time.
Fawkes library namespace.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).