fawkes_skiller_node.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *  fawkes_skiller_node - Skiller interface similar to Fawkes' ros-skiller
00003  *
00004  *  Created: Tue May 31 21:24:09 2016
00005  *  Copyright  2016  Tim Niemueller [www.niemueller.de]
00006  ****************************************************************************/
00007 
00008 /*  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU Library General Public License for more details.
00017  *
00018  *  Read the full text in the LICENSE.GPL file in the doc directory.
00019  */
00020 
00021 
00022 #include <ros/ros.h>
00023 #include <ros/callback_queue.h>
00024 
00025 #include <blackboard/remote.h>
00026 #include <interfaces/SkillerInterface.h>
00027 #include <utils/time/time.h>
00028 
00029 #include <actionlib/server/simple_action_server.h>
00030 #include <fawkes_msgs/ExecSkillAction.h>
00031 #include <fawkes_msgs/ExecSkillGoal.h>
00032 #include <fawkes_msgs/ExecSkillActionGoal.h>
00033 #include <fawkes_msgs/SkillStatus.h>
00034 
00035 #include <memory>
00036 
00037 #include <mutex>
00038 
00039 #define GET_PRIV_PARAM(P)         \
00040         { \
00041                 if (! ros::param::get("~" #P, cfg_ ## P ## _)) { \
00042                         ROS_ERROR("%s: Failed to retrieve parameter " #P ", aborting", ros::this_node::getName().c_str()); \
00043                         exit(-1); \
00044                 } \
00045         }
00046 
00047 using namespace fawkes;
00048 
00049 typedef actionlib::ActionServer<fawkes_msgs::ExecSkillAction> SkillerServer;
00050 
00051 
00052 class RosSkillerNode
00053 {
00054  public:
00055         RosSkillerNode(ros::NodeHandle *rn, std::shared_ptr<fawkes::BlackBoard> bb)
00056                 : rosnode(rn), blackboard(bb) {}
00057 
00058         fawkes_msgs::ExecSkillResult
00059         create_result(std::string errmsg)
00060         {
00061                 fawkes_msgs::ExecSkillResult result;
00062                 result.errmsg = errmsg;
00063                 return result;
00064         }
00065 
00066 
00067         fawkes_msgs::ExecSkillFeedback
00068         create_feedback()
00069         {
00070                 return fawkes_msgs::ExecSkillFeedback();
00071         }
00072 
00073         void
00074         action_goal_cb(SkillerServer::GoalHandle goal)
00075         {
00076                 ROS_INFO("%s: Received goal to execute %s", ros::this_node::getName().c_str(),
00077                          goal.getGoal()->skillstring.c_str());
00078                 std::lock_guard<std::mutex> lock(loop_mutex);
00079                 if (exec_running_ && exec_as_) {
00080                         std::string error_msg = "Replaced by new goal";
00081                         as_goal_.setAborted(create_result(error_msg), error_msg);
00082                 }
00083     if (! blackboard || !blackboard->is_alive()) {
00084       std::string error_msg = "Blackboard not connected";
00085       goal.setAborted(create_result(error_msg), error_msg);
00086       return;
00087     }
00088                 as_goal_      = goal;
00089                 goal_         = goal.getGoal()->skillstring;
00090                 exec_request_ = true;
00091                 exec_as_      = true;
00092 
00093                 ROS_INFO("%s: Accepting goal '%s'", ros::this_node::getName().c_str(), goal_.c_str());
00094                 goal.setAccepted();
00095         }
00096 
00097         void
00098         action_cancel_cb(SkillerServer::GoalHandle goal)
00099         {
00100                 std::lock_guard<std::mutex> lock(loop_mutex);
00101                 stop();
00102                 std::string error_msg = "Abort on request";
00103                 goal.setCanceled(create_result(error_msg), error_msg);
00104         }
00105 
00106         virtual void init()
00107         {
00108                 exec_request_ = false;
00109                 exec_running_ = false;
00110                 exec_as_      = false;
00111                 wait_release_ = 0;
00112                 
00113                 try {
00114                         skiller_if_ = blackboard->open_for_reading<SkillerInterface>("Skiller");
00115                 } catch (const Exception &e) {
00116                         ROS_ERROR("%s: Initialization failed, could not open Skiller interface", ros::this_node::getName().c_str());
00117                         throw;
00118                 }
00119 
00120                 server_ = new SkillerServer(*rosnode, "skiller",
00121                                             boost::bind(&RosSkillerNode::action_goal_cb, this, _1),
00122                                             boost::bind(&RosSkillerNode::action_cancel_cb, this, _1),
00123                                             /* auto_start */ false);
00124 
00125                 pub_status_ = rosnode->advertise<fawkes_msgs::SkillStatus>("skiller_status", true);
00126 
00127                 server_->start();
00128         }
00129 
00130         virtual void finalize()
00131         {
00132                 try {
00133                         blackboard->close(skiller_if_);
00134                 } catch (Exception& e) {
00135                         ROS_ERROR("%s: Closing interface failed!", ros::this_node::getName().c_str());
00136                 }
00137                 pub_status_.shutdown();
00138                 delete server_;
00139         }
00140 
00141         void
00142         stop()
00143         {
00144                 if (skiller_if_->exclusive_controller() != skiller_if_->serial()) {
00145                         ROS_ERROR("%s: Skill abortion requested, but not in control", ros::this_node::getName().c_str());
00146                         return;
00147                 }
00148 
00149                 if (skiller_if_->has_writer())
00150                         skiller_if_->msgq_enqueue(new SkillerInterface::StopExecMessage());
00151                 if (exec_as_) {
00152                         std::string error_msg = "Abort on request";
00153                         as_goal_.setAborted(create_result(error_msg), error_msg);
00154                 }
00155                 skiller_if_->msgq_enqueue(new SkillerInterface::ReleaseControlMessage());
00156                 exec_running_ = false;
00157         }
00158 
00159         void
00160         loop()
00161         {
00162                 std::lock_guard<std::mutex> lock(loop_mutex);
00163                 skiller_if_->read();
00164 
00165                 // currently idle, release skiller control
00166                 if (!exec_running_ && !exec_request_ && wait_release_ == 0 &&
00167                     skiller_if_->exclusive_controller() == skiller_if_->serial())
00168                 {
00169                         ROS_INFO("%s: No skill running and no skill requested, releasing control",
00170                                  ros::this_node::getName().c_str());
00171                         skiller_if_->msgq_enqueue(new SkillerInterface::ReleaseControlMessage());
00172                         wait_release_ = 1;
00173                         return;
00174                 }
00175 
00176                 if (wait_release_ > 0 &&
00177                     skiller_if_->exclusive_controller() == skiller_if_->serial())
00178                 {
00179                         if (++wait_release_ % 50 == 0) {
00180                                 ROS_WARN("%s: waiting for exclusive control release", 
00181                                          ros::this_node::getName().c_str());
00182                         }
00183                         ros::WallDuration(0.1).sleep();
00184                         if (wait_release_ > 30) {
00185                                 // Reset, will triggering another release
00186                                 wait_release_ = 0;
00187                         }
00188                         return;
00189                 }
00190 
00191                 if (exec_request_) {
00192                         if (!skiller_if_->has_writer()) {
00193                                 std::string error_msg("no writer for skiller, cannot execute");
00194                                 ROS_WARN("%s: %s", ros::this_node::getName().c_str(), error_msg.c_str());
00195                                 stop();
00196                                 as_goal_.setAborted(create_result(error_msg), error_msg);
00197                                 return;
00198                         }
00199 
00200                         if (skiller_if_->exclusive_controller() != skiller_if_->serial()) {
00201                                 // we need the skiller control, acquire it first
00202                                 ROS_INFO("%s: Skill execution requested, acquiring control", ros::this_node::getName().c_str());
00203                                 skiller_if_->msgq_enqueue(new SkillerInterface::AcquireControlMessage());
00204                                 return;
00205                         }
00206                         exec_request_ = false;
00207 
00208                         SkillerInterface::ExecSkillMessage *msg =
00209                                 new SkillerInterface::ExecSkillMessage(goal_.c_str());
00210                         msg->ref();
00211 
00212                         ROS_INFO("%s: Creating goal '%s'", ros::this_node::getName().c_str(), goal_.c_str());
00213 
00214                         try {
00215                                 skiller_if_->msgq_enqueue(msg);
00216                                 exec_running_ = true;
00217                                 exec_msgid_ = msg->id();
00218                                 exec_skill_string_ = msg->skill_string();
00219                                 loops_waited_ = 0;
00220                         } catch (Exception &e) {
00221                                 ROS_WARN("%s: Failed to execute skill, exception %s", ros::this_node::getName().c_str(), e.what_no_backtrace());
00222                         }
00223                         msg->unref();
00224 
00225                 } else if (exec_running_) {
00226                         if (exec_as_) as_goal_.publishFeedback(create_feedback());
00227 
00228                         if (skiller_if_->status() == SkillerInterface::S_INACTIVE ||
00229                             skiller_if_->msgid() != exec_msgid_)
00230                         {
00231                                 // wait three loops, maybe the skiller will start
00232                                 //ROS_INFO("Should be executing skill, but skiller is inactive or processing other message");
00233                                 ++loops_waited_;
00234                                 if (loops_waited_ >= 5) {
00235                                         // give up and abort
00236                                         ROS_WARN("%s: Skiller doesn't start, aborting", ros::this_node::getName().c_str());
00237                                         std::string error_msg =  "Skiller doesn't start";
00238                                         if (exec_as_) as_goal_.setAborted(create_result(error_msg), error_msg);
00239                                         exec_running_ = false;
00240                                 }
00241                         }
00242                         else if (skiller_if_->status() != SkillerInterface::S_RUNNING) {
00243                                 exec_running_ = false;
00244                                 if (exec_as_ && exec_skill_string_ == skiller_if_->skill_string()) {
00245                                         if (skiller_if_->status() == SkillerInterface::S_FINAL) {
00246                                                 std::string error_msg = "Skill executed";
00247                                                 as_goal_.setSucceeded(create_result(error_msg), error_msg);
00248                                         }
00249                                         else if (skiller_if_->status() == SkillerInterface::S_FAILED){
00250                                                 std::string error_msg = "Failed to execute skill";
00251                                                 char * tmp;
00252                                                 if (asprintf(&tmp, "Failed to execute skill, error: %s", skiller_if_->error()) != -1) {
00253                                                         error_msg = tmp;
00254                                                         free(tmp);
00255                                                 }
00256                                                 as_goal_.setAborted(create_result(error_msg), error_msg);
00257                                         }
00258                                         else {
00259                                                 ROS_WARN("%s: Don't know what happened, but not running", ros::this_node::getName().c_str());
00260                                         }
00261                                 }
00262                         }
00263                 }
00264 
00265                 if (skiller_if_->changed()) {
00266                         fawkes_msgs::SkillStatus msg;
00267                         const Time *time = skiller_if_->timestamp();
00268                         msg.stamp        = ros::Time(time->get_sec(), time->get_nsec());
00269                         msg.skill_string = skiller_if_->skill_string();
00270                         msg.error        = skiller_if_->error();
00271                         msg.status       = skiller_if_->status();
00272                         pub_status_.publish(msg);
00273                 }
00274         }
00275 
00276  private:
00277         ros::NodeHandle *rosnode;
00278         std::shared_ptr<fawkes::BlackBoard> blackboard;
00279 
00280   std::mutex loop_mutex;
00281 
00282   fawkes::SkillerInterface *skiller_if_;
00283 
00284   SkillerServer   *server_;
00285   ros::Publisher   pub_status_;
00286 
00287   SkillerServer::GoalHandle as_goal_;
00288   std::string goal_;
00289 
00290   bool         exec_as_;
00291   bool         exec_request_;
00292   bool         exec_running_;
00293   unsigned int exec_msgid_;
00294   std::string exec_skill_string_;
00295   unsigned int loops_waited_;
00296 
00297         unsigned int wait_release_;
00298 };
00299 
00300 
00301 int
00302 main(int argc, char **argv)
00303 {
00304         ros::init(argc, argv, "fawkes_skiller_forward");
00305 
00306         ros::NodeHandle n;
00307 
00308         std::shared_ptr<RosSkillerNode> ros_skiller_node;
00309         std::string  cfg_fawkes_host_;
00310         int          cfg_fawkes_port_;
00311         std::shared_ptr<fawkes::BlackBoard> blackboard_;
00312 
00313         GET_PRIV_PARAM(fawkes_host);
00314         GET_PRIV_PARAM(fawkes_port);
00315 
00316         while (ros::ok()) {
00317                 if (!blackboard_) {
00318                         ROS_WARN_THROTTLE(30, "%s: Not connected, retrying", ros::this_node::getName().c_str());
00319                         try {
00320                                 blackboard_ =
00321                                         std::make_shared<fawkes::RemoteBlackBoard>(cfg_fawkes_host_.c_str(), cfg_fawkes_port_);
00322                                 ros_skiller_node = std::make_shared<RosSkillerNode>(&n, blackboard_);
00323                                 ros_skiller_node->init();
00324                                 ROS_INFO("%s: Blackboard connected and initialized", ros::this_node::getName().c_str());
00325                         } catch (fawkes::Exception &e) {
00326                                 ROS_WARN_THROTTLE(10, "%s: Initialization failed, retrying", ros::this_node::getName().c_str());
00327                                 if (ros_skiller_node) {
00328                                         ros_skiller_node->finalize();
00329                                 }
00330                                 ros_skiller_node.reset();
00331                                 blackboard_.reset();
00332                         }
00333                 } else if (! blackboard_->is_alive()) {
00334                         ROS_WARN_THROTTLE(30, "%s: blackboard connection lost, retrying", ros::this_node::getName().c_str());
00335                         if (blackboard_->try_aliveness_restore()) {
00336                                 ROS_INFO("%s: Blackboard re-connected", ros::this_node::getName().c_str());
00337                         }
00338                 } else {
00339                         ros_skiller_node->loop();
00340                 }
00341 
00342                 ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1));
00343         }
00344 
00345         if (ros_skiller_node) ros_skiller_node->finalize();
00346 
00347         return 0;
00348 }


rcll_fawkes_sim
Author(s): Tim Niemueller
autogenerated on Thu Jun 6 2019 17:48:58