Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031
00032 #include <actionlib/client/simple_action_client.h>
00033 #include <move_base_msgs/MoveBaseAction.h>
00034 #include <nav_msgs/Path.h>
00035
00036 #include <neonavigation_common/compatibility.h>
00037
00038 class PatrolActionNode
00039 {
00040 protected:
00041 using MoveBaseClient = actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>;
00042
00043 ros::NodeHandle nh_;
00044 ros::NodeHandle pnh_;
00045
00046 ros::Subscriber sub_path_;
00047 std::shared_ptr<MoveBaseClient> act_cli_;
00048
00049 nav_msgs::Path path_;
00050 size_t pos_;
00051
00052 void cbPath(const nav_msgs::Path::ConstPtr& msg)
00053 {
00054 path_ = *msg;
00055 pos_ = 0;
00056 }
00057
00058 public:
00059 PatrolActionNode()
00060 : nh_()
00061 , pnh_("~")
00062 {
00063 neonavigation_common::compat::checkCompatMode();
00064 sub_path_ = neonavigation_common::compat::subscribe(
00065 nh_, "patrol_nodes",
00066 pnh_, "path", 1, &PatrolActionNode::cbPath, this);
00067 act_cli_.reset(new MoveBaseClient("move_base", false));
00068
00069 pos_ = 0;
00070 }
00071 bool sendNextGoal()
00072 {
00073 move_base_msgs::MoveBaseGoal goal;
00074
00075 if (path_.poses.size() <= pos_)
00076 {
00077 ROS_WARN("Patrol finished. Waiting next path.");
00078 path_.poses.clear();
00079
00080 return false;
00081 }
00082
00083 goal.target_pose.header = path_.poses[pos_].header;
00084 goal.target_pose.header.stamp = ros::Time::now();
00085 goal.target_pose.pose = path_.poses[pos_].pose;
00086
00087 act_cli_->sendGoal(goal);
00088 pos_++;
00089
00090 return true;
00091 }
00092 void spin()
00093 {
00094 ros::Rate rate(1.0);
00095
00096 while (ros::ok())
00097 {
00098 ros::spinOnce();
00099 rate.sleep();
00100
00101 if (path_.poses.size() == 0)
00102 {
00103 continue;
00104 }
00105
00106 if (pos_ == 0)
00107 {
00108 sendNextGoal();
00109 continue;
00110 }
00111
00112 actionlib::SimpleClientGoalState state = act_cli_->getState();
00113 if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
00114 {
00115 ROS_INFO("Action has been finished.");
00116 sendNextGoal();
00117 }
00118 else if (state == actionlib::SimpleClientGoalState::ABORTED)
00119 {
00120 ROS_ERROR("Action has been aborted. Skipping.");
00121 sendNextGoal();
00122 }
00123 else if (state == actionlib::SimpleClientGoalState::LOST)
00124 {
00125 ROS_WARN_ONCE("Action server is not ready.");
00126 }
00127 }
00128 }
00129 };
00130
00131 int main(int argc, char** argv)
00132 {
00133 ros::init(argc, argv, "patrol");
00134
00135 PatrolActionNode pa;
00136 pa.spin();
00137
00138 return 0;
00139 }