uav_random_direction.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <ros/console.h>
4 #include <cpswarm_msgs/CoverageAction.h>
5 #include <cpswarm_msgs/TargetPositionEvent.h>
7 
8 using namespace ros;
9 
14 
18 cpswarm_msgs::CoverageResult result;
19 
24 
30 void ActionCallback(const cpswarm_msgs::CoverageGoalConstPtr& goal, action_server_t* as)
31 {
32  NodeHandle nh;
33 
34  // set loop rate
35  double loop_rate;
36  nh.param(this_node::getName() + "/loop_rate", loop_rate, 5.0);
37  Rate rate(loop_rate);
38 
39  ROS_INFO("Executing coverage");
40 
41  // coverage library
42  uav_random_direction uav_coverage(goal->altitude);
43 
44  // execute coverage until state changes
46  while (ok() && !as->isPreemptRequested() && state == STATE_ACTIVE) {
47  ROS_DEBUG("Coverage step");
48  behavior_state_t result = uav_coverage.step();
49  if (state == STATE_ACTIVE)
50  state = result;
51  rate.sleep();
52  spinOnce();
53  }
54 
55  // stop moving
56  uav_coverage.stop();
57 
58  // coverage succeeded
59  if (state == STATE_SUCCEEDED) {
60  ROS_INFO("Coverage succeeded, found target %d at [%f, %f]", result.target_id, result.target_pose.pose.position.x, result.target_pose.pose.position.y);
61 
62  as->setSucceeded(result);
63  }
64 
65  // coverage aborted
66  else if (state == STATE_ABORTED) {
67  ROS_INFO("Coverage aborted");
68  as->setAborted();
69  }
70 
71  // coverage was preempted
72  else{
73  ROS_INFO("Coverage preempted");
74  as->setPreempted();
75  }
76 }
77 
82 void found_callback (const cpswarm_msgs::TargetPositionEvent::ConstPtr& msg)
83 {
85  result.target_id = msg->id;
86  result.target_pose = msg->pose;
87 }
88 
95 int main (int argc, char** argv)
96 {
97  // init ros node
98  init(argc, argv, "uav_coverage");
99  NodeHandle nh;
100 
101  // define which log messages are shown
104  }
105  else{
106  ROS_ERROR("Could not set logger level!");
107  }
108 
109  // stop covering once a target has been found
110  int queue_size;
111  nh.param(this_node::getName() + "/queue_size", queue_size, 1);
112  bool single_target;
113  nh.param(this_node::getName() + "/single_target", single_target, true);
114  Subscriber found_sub;
115  if (single_target)
116  found_sub = nh.subscribe("target_found", queue_size, found_callback);
117 
118  // start action server
119  action_server_t as(nh, "uav_coverage", boost::bind(&ActionCallback, _1, &as), false);
120  as.start();
121 
122  // wait for action client
123  spin();
124 
125  return 0;
126 }
127 
void found_callback(const cpswarm_msgs::TargetPositionEvent::ConstPtr &msg)
Callback function to receive details of a target that has been detected.
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
behavior_state_t
An enumeration for the state of the behavior algorithm.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
actionlib::SimpleActionServer< cpswarm_msgs::CoverageAction > action_server_t
An action server type that allows to start and stop the coverage task.
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
cpswarm_msgs::CoverageResult result
The target found during execution of the coverage algorithm.
ROSCPP_DECL bool ok()
behavior_state_t state
The state of the behavior algorithm.
behavior_state_t step()
Move the swarm member to a new position.
ROSCPP_DECL void spin()
bool sleep()
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
void ActionCallback(const cpswarm_msgs::CoverageGoalConstPtr &goal, action_server_t *as)
Callback of the action server which executes the coverage task until it is preempted or finished...
int main(int argc, char **argv)
Main function to be executed by ROS.
A class that allows to cover a given area with the random direction algorithm. The random direction i...
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
#define ROSCONSOLE_DEFAULT_NAME
#define ROS_DEBUG(...)


uav_random_direction
Author(s): Micha Sende
autogenerated on Sat Feb 6 2021 03:11:41