00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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 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
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
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
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
00232
00233 ++loops_waited_;
00234 if (loops_waited_ >= 5) {
00235
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 }