Changes have been pushed for the project "Fawkes Robotics Software Framework".
Gitweb: http://git.fawkesrobotics.org/fawkes.git Trac: http://trac.fawkesrobotics.org The branch, jrothe/navigator_thread_merge_fix has been created at 4860537cec245a33eb40e998745989f9edbb2cb5 (commit) http://git.fawkesrobotics.org/fawkes.git/jrothe/navigator_thread_merge_fix - *Log* --------------------------------------------------------------- commit 4860537cec245a33eb40e998745989f9edbb2cb5 Author: Johannes Rothe <johannes.ro...@rwth-aachen.de> AuthorDate: Wed May 29 13:21:11 2013 +0200 Commit: Johannes Rothe <johannes.ro...@rwth-aachen.de> CommitDate: Wed May 29 13:21:11 2013 +0200 Ros Navigator_thread: stop_goals was lost at merge Copied it from the backup http://git.fawkesrobotics.org/fawkes.git/commit/4860537 http://trac.fawkesrobotics.org/changeset/4860537 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- - *Diffs* ------------------------------------------------------------- - *commit* 4860537cec245a33eb40e998745989f9edbb2cb5 - - - - - - - - - - Author: Johannes Rothe <johannes.ro...@rwth-aachen.de> Date: Wed May 29 13:21:11 2013 +0200 Subject: Ros Navigator_thread: stop_goals was lost at merge src/plugins/ros/navigator_thread.cpp | 35 ++++++++++++++++++++++++++++++--- src/plugins/ros/navigator_thread.h | 1 + 2 files changed, 32 insertions(+), 4 deletions(-) _Diff for modified files_: diff --git a/src/plugins/ros/navigator_thread.cpp b/src/plugins/ros/navigator_thread.cpp index be93846..e28dcb9 100644 --- a/src/plugins/ros/navigator_thread.cpp +++ b/src/plugins/ros/navigator_thread.cpp @@ -110,6 +110,13 @@ RosNavigatorThread::send_goal() } void +RosNavigatorThread::stop_goals() +{ + //cancel all existing goals and send Goal={0/0/0} + ac_->cancelAllGoals(); +} + +void RosNavigatorThread::loop() { if (! ac_->isServerConnected()) { @@ -139,6 +146,10 @@ RosNavigatorThread::loop() nav_if_->set_dest_ori(0); nav_if_->set_msgid(msg->id()); + nav_if_->msgq_pop(); + nav_if_->write(); + + stop_goals(); } // cartesian goto @@ -150,6 +161,11 @@ RosNavigatorThread::loop() nav_if_->set_dest_ori(msg->orientation()); nav_if_->set_msgid(msg->id()); + + nav_if_->msgq_pop(); + nav_if_->write(); + + send_goal(); } // polar goto @@ -161,6 +177,11 @@ RosNavigatorThread::loop() nav_if_->set_dest_ori(msg->phi()); nav_if_->set_msgid(msg->id()); + + nav_if_->msgq_pop(); + nav_if_->write(); + + send_goal(); } // max velocity @@ -169,6 +190,11 @@ RosNavigatorThread::loop() nav_if_->set_max_velocity(msg->max_velocity()); nav_if_->set_msgid(msg->id()); + + nav_if_->msgq_pop(); + nav_if_->write(); + + send_goal(); } else if (NavigatorInterface::SetSecurityDistanceMessage *msg = @@ -178,12 +204,13 @@ RosNavigatorThread::loop() nav_if_->set_security_distance (msg->security_distance ()); nav_if_->set_msgid(msg->id()); - } + + nav_if_->msgq_pop(); + nav_if_->write(); - nav_if_->msgq_pop(); - nav_if_->write(); + send_goal(); + } - send_goal(); } // while check_status(); diff --git a/src/plugins/ros/navigator_thread.h b/src/plugins/ros/navigator_thread.h index 808b942..2bae738 100644 --- a/src/plugins/ros/navigator_thread.h +++ b/src/plugins/ros/navigator_thread.h @@ -60,6 +60,7 @@ class RosNavigatorThread private: void check_status(); void send_goal(); + void stop_goals(); private: typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; -- Fawkes Robotics Framework http://www.fawkesrobotics.org _______________________________________________ fawkes-commits mailing list fawkes-commits@lists.kbsg.rwth-aachen.de https://lists.kbsg.rwth-aachen.de/listinfo/fawkes-commits
