lib/uav_optimal_coverage.cpp
Go to the documentation of this file.
2 
4 {
5  NodeHandle nh;
6 
7  // init state of algorithm
9 
10  // read parameters
11  nh.param(this_node::getName() + "/target_velocity", target_velocity, 0.5);
12  int queue_size;
13  nh.param(this_node::getName() + "/queue_size", queue_size, 1);
14  nh.param(this_node::getName() + "/tolerance", tolerance, 0.5);
15 
16  // publishers, subscribers, and service clients
17  wp_getter = nh.serviceClient<cpswarm_msgs::GetWaypoint>("coverage_path/waypoint");
18  wp_getter.waitForExistence();
19 
20  // get initial waypoint of path
21  get_wp.request.position = pos.get_pose().position;
22  get_wp.request.tolerance = tolerance;
23  if (wp_getter.call(get_wp) == false || get_wp.response.valid == false){
24  ROS_ERROR("Failed to get waypoint, cannot perform coverage!");
26  return;
27  }
28  waypoint = get_wp.response.point;
29  ROS_INFO("Initial waypoint [%.2f, %.2f]", waypoint.x, waypoint.y);
30 }
31 
33 {
34 }
35 
37 {
38  // update position information
39  spinOnce();
40 
41  if (state == STATE_ACTIVE) {
42  // reached current waypoint of path
43  if (hypot(waypoint.x - pos.get_pose().position.x, waypoint.y - pos.get_pose().position.y) < tolerance) {
44  // get next waypoint of path
45  get_wp.request.position = pos.get_pose().position;
46  get_wp.request.tolerance = tolerance;
47  if (wp_getter.call(get_wp) == false) {
48  ROS_ERROR("Failed to get waypoint, cannot perform coverage!");
49  return STATE_ABORTED;
50  }
51  waypoint = get_wp.response.point;
52  ROS_INFO("Move to waypoint [%.2f, %.2f]", waypoint.x, waypoint.y);
53  }
54 
55  // convert waypoint to pose
56  geometry_msgs::Pose goal = pos.get_pose();
57  goal.position.x = waypoint.x;
58  goal.position.y = waypoint.y;
59 
60  // finished path
61  if (get_wp.response.valid == false) {
62  ROS_INFO("Path completely traversed, stop coverage!");
64  }
65 
66  // new goal is out of bounds
67  else if (pos.out_of_bounds(goal)) {
68  ROS_ERROR("Waypoint out of allowed area, stop coverage!");
70  }
71 
72  // obstacle in direction of new goal
73  else if (pos.occupied(goal)) {
74  ROS_ERROR("Obstacle ahead, stop coverage!");
76  }
77 
78  // move to new position
79  else if (pos.move(goal) == false)
81  }
82 
83  // return state to action server
84  return state;
85 }
86 
88 {
89  geometry_msgs::Vector3 velocity;
90  vel.move(velocity);
91 }
geometry_msgs::Point waypoint
Current waypoint to navigate to.
uav_optimal_coverage()
Constructor that initializes the private member variables.
double target_velocity
Target velocity of the UAV.
velocity vel
A helper object for velocity related tasks.
ServiceClient wp_getter
Service client to get the current waypoint to navigate to.
~uav_optimal_coverage()
Destructor that deletes the private member objects.
behavior_state_t state
The state of the behavior algorithm.
behavior_state_t step()
Move the swarm member to a new position.
cpswarm_msgs::GetWaypoint get_wp
Service message to get the current waypoint.
behavior_state_t
An enumeration for the state of the behavior algorithm.
position pos
A helper object for position related tasks.


uav_optimal_coverage
Author(s): Micha Sende
autogenerated on Sun Dec 29 2019 10:25:49