position.cpp
Go to the documentation of this file.
1 #include "position.h"
2 
3 position::position (double altitude) : altitude(altitude)
4 {
5  // read parameters
6  double loop_rate;
7  nh.param(this_node::getName() + "/loop_rate", loop_rate, 5.0);
8  rate = new Rate(loop_rate);
9  int queue_size;
10  nh.param(this_node::getName() + "/queue_size", queue_size, 1);
11  nh.param(this_node::getName() + "/goal_tolerance", goal_tolerance, 0.1);
12  double d_move_timeout;
13  nh.param(this_node::getName() + "/move_timeout", d_move_timeout, 10.0);
14  move_timeout = Duration(d_move_timeout);
15  nh.param(this_node::getName() + "/visualize", visualize, false);
16 
17  // no pose received yet
18  pose_valid = false;
19 
20  // init ros communication
21  out_of_bounds_client = nh.serviceClient<cpswarm_msgs::OutOfBounds>("area/out_of_bounds");
23  occupied_sector_client = nh.serviceClient<cpswarm_msgs::GetSector>("obstacle_detection/get_occupied_sector");
25  pose_sub = nh.subscribe("pos_provider/pose", queue_size, &position::pose_callback, this);
26  pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pos_controller/goal_position", queue_size, true);
27  if (visualize)
28  visualize_pub = nh.advertise<geometry_msgs::PointStamped>("swarm_behaviors_position/goal", queue_size, true);
29 
30  // init position and yaw
31  while (ok() && pose_valid == false) {
32  ROS_DEBUG_ONCE("Waiting for valid pose...");
33  rate->sleep();
34  spinOnce();
35  }
36 
37  // init goal
38  goal.pose = pose;
39 }
40 
42 {
43  delete rate;
44 }
45 
46 double position::bearing (geometry_msgs::Pose p) const
47 {
48  double b = remainder(atan2(p.position.y - pose.position.y, p.position.x - pose.position.x) - get_yaw(), 2*M_PI);
49  if (b < 0)
50  b += 2*M_PI;
51  return b;
52 }
53 
54 geometry_msgs::Pose position::compute_goal (double distance, double direction) const
55 {
56  return compute_goal(pose, distance, direction);
57 }
58 
59 geometry_msgs::Pose position::compute_goal (geometry_msgs::Pose start, double distance, double direction) const
60 {
61  geometry_msgs::Pose goal;
62 
63  // calculate position
64  goal.position.x = start.position.x + distance * cos(direction);
65  goal.position.y = start.position.y + distance * sin(direction);
66  goal.position.z = altitude;
67 
68  // calculate orientation
69  tf2::Quaternion orientation;
70  orientation.setRPY(0, 0, direction);
71  goal.orientation = tf2::toMsg(orientation);
72 
73  return goal;
74 }
75 
76 double position::dist (geometry_msgs::Pose p) const
77 {
78  return dist(pose, p);
79 }
80 
81 double position::dist (geometry_msgs::Pose p1, geometry_msgs::Pose p2) const
82 {
83  return hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y);
84 }
85 
86 geometry_msgs::Pose position::get_pose () const
87 {
88  return pose;
89 }
90 
91 double position::get_tolerance () const
92 {
93  return goal_tolerance;
94 }
95 
96 double position::get_yaw () const
97 {
98  return get_yaw(pose);
99 }
100 
101 bool position::move (geometry_msgs::Pose goal)
102 {
103  // goal is out of bounds
104  if (out_of_bounds(goal)) {
105  ROS_ERROR("Cannot move to (%.2f,%.2f) because it is out of bounds!", goal.position.x, goal.position.y);
106  stop();
107  return false;
108  }
109 
110  // obstacle in direction of goal
111  else if (occupied(goal)) {
112  ROS_ERROR("Obstacle ahead, stop moving!");
113  stop();
114  return false;
115  }
116 
117  // goal changed
118  if (this->goal.header.stamp.isZero() || this->goal.pose.position.x != goal.position.x || this->goal.pose.position.y != goal.position.y) {
119  // visualize goal
120  if (visualize) {
121  geometry_msgs::PointStamped wp;
122  wp.header.stamp = Time::now();
123  wp.header.frame_id = "local_origin_ned";
124  wp.point = goal.position;
126  }
127 
128  // create goal pose
129  this->goal.header.stamp = Time::now();
130  this->goal.pose = goal;
131  this->goal.pose.position.z = altitude;
132 
133  // send goal pose to cps controller
134  pose_pub.publish(this->goal);
135  }
136 
137  // successfully published goal set point
138  return true;
139 }
140 
141 bool position::occupied (geometry_msgs::Pose pose)
142 {
143  // get occupied sector
144  cpswarm_msgs::GetSector ocs;
145  if (occupied_sector_client.call(ocs)) {
146  // no obstacles
147  if (ocs.response.min == ocs.response.max)
148  return false;
149 
150  ROS_DEBUG("Goal %.2f in occupied sector [%.2f,%.2f]?", bearing(pose), ocs.response.min, ocs.response.max);
151 
152  // check if pose is in this sector
153  if (ocs.response.max > 2*M_PI && bearing(pose) < M_PI)
154  return ocs.response.min <= bearing(pose) + 2*M_PI && bearing(pose) + 2*M_PI <= ocs.response.max; // always max > min
155  else
156  return ocs.response.min <= bearing(pose) && bearing(pose) <= ocs.response.max; // always max > min
157  }
158  else {
159  ROS_ERROR("Failed to check if goal is occupied");
160  return true;
161  }
162 }
163 
164 bool position::out_of_bounds (geometry_msgs::Pose pose)
165 {
166  cpswarm_msgs::OutOfBounds oob;
167  oob.request.pose = pose;
168  if (out_of_bounds_client.call(oob)) {
169  return oob.response.out;
170  }
171  else {
172  ROS_ERROR("Failed to check if goal is out of bounds");
173  return true;
174  }
175 }
176 
178 {
179  ROS_DEBUG("Yaw %.2f --> %.2f", get_yaw(pose), get_yaw(goal.pose));
180  ROS_DEBUG("Pose (%.2f,%.2f) --> (%.2f,%.2f)", pose.position.x, pose.position.y, goal.pose.position.x, goal.pose.position.y);
181  ROS_DEBUG("%.2f > %.2f", dist(pose, goal.pose), goal_tolerance);
182 
183  // invalid goal
184  if (goal.header.stamp.isZero())
185  return true;
186 
187  // time out
188  if (goal.header.stamp + move_timeout < Time::now()) {
189  ROS_ERROR("Could not reach goal within timeout %.2fs", move_timeout.toSec());
190 
191  // invalidate goal
192  goal.header.stamp = Time(0);
193 
194  // let behavior think goal is reached
195  return true;
196  }
197 
198  // whether cps reached goal position, ignoring yaw
199  return dist(pose, goal.pose) <= goal_tolerance;
200 }
201 
203 {
204  // create goal pose
205  goal.header.stamp = Time::now();
206  goal.pose = pose;
207  goal.pose.position.z = altitude;
208 
209  // send goal pose to cps controller
211  ROS_INFO("Stopping in [%.2f, %.2f]", goal.pose.position.x, goal.pose.position.y);
212 
213  // sleep to guarantee that goal is published
214  sleep(0.01);
215 }
216 
217 double position::get_yaw (geometry_msgs::Pose pose) const
218 {
219  tf2::Quaternion orientation;
220  tf2::fromMsg(pose.orientation, orientation);
221  return tf2::getYaw(orientation);
222 }
223 
224 void position::pose_callback (const geometry_msgs::PoseStamped::ConstPtr& msg)
225 {
226  // valid pose received
227  if (msg->header.stamp.isValid())
228  pose_valid = true;
229 
230  // store new position and orientation in class variables
231  pose = msg->pose;
232 
233  ROS_DEBUG_THROTTLE(1, "Yaw %.2f", get_yaw());
234  ROS_DEBUG_THROTTLE(1, "Pose [%.2f, %.2f, %.2f]", pose.position.x, pose.position.y, pose.position.z);
235 }
double bearing(geometry_msgs::Pose p) const
Compute the bearing from the current pose of the CPS to a given pose.
Definition: position.cpp:46
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
Publisher visualize_pub
Publisher to visualize the current goal.
Definition: position.h:156
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
double altitude
The altitude at which the CPS operates.
Definition: position.h:201
bool move(geometry_msgs::Pose pose)
Move the CPS to the given pose.
Definition: position.cpp:101
NodeHandle nh
A node handle for the main ROS node.
Definition: position.h:161
position(double altitude)
Constructor that initializes the private member variables.
Definition: position.cpp:3
void publish(const boost::shared_ptr< M > &message) const
Rate * rate
The loop rate object for running the behavior control loops at a specific frequency.
Definition: position.h:166
~position()
Destructor that deletes the private member objects.
Definition: position.cpp:41
#define ROS_DEBUG_THROTTLE(rate,...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool pose_valid
Whether a valid position has been received.
Definition: position.h:181
geometry_msgs::PoseStamped goal
Current goal of the CPS.
Definition: position.h:176
bool reached()
Check whether the CPS has reached the current goal.
Definition: position.cpp:177
bool call(MReq &req, MRes &res)
#define ROS_DEBUG_ONCE(...)
bool occupied(geometry_msgs::Pose pose)
Check whether there is an obstacle in the direction of the given pose.
Definition: position.cpp:141
geometry_msgs::Pose pose
Current position of the CPS.
Definition: position.h:171
double goal_tolerance
The distance that the CPS can be away from a goal while still being considered to have reached that g...
Definition: position.h:186
void pose_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
Callback function for position updates.
Definition: position.cpp:224
double get_yaw() const
Get the current yaw orientation of the CPS.
Definition: position.cpp:96
Publisher pose_pub
Publisher for sending the goal position of the CPS to the position controller in the abstraction libr...
Definition: position.h:151
geometry_msgs::Pose get_pose() const
Get the current pose of the CPS.
Definition: position.cpp:86
double get_tolerance() const
Get the position tolerance.
Definition: position.cpp:91
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void fromMsg(const A &, B &b)
ROSCPP_DECL bool ok()
ServiceClient occupied_sector_client
Service client for determining the sector occupied by obstacles.
Definition: position.h:146
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
geometry_msgs::Pose compute_goal(double distance, double direction) const
Compute the goal coordinates relative to the current position.
Definition: position.cpp:54
bool sleep()
double getYaw(const A &a)
B toMsg(const A &a)
bool out_of_bounds(geometry_msgs::Pose pose)
Check whether a given pose is out of the mission area boundaries.
Definition: position.cpp:164
void stop()
Stop moving by publishing the current position as goal.
Definition: position.cpp:202
Subscriber pose_sub
Subscriber for the position of the CPS.
Definition: position.h:136
double dist(geometry_msgs::Pose p) const
Compute the straight-line distance from the current position to the given position.
Definition: position.cpp:76
ROSCPP_DECL void spinOnce()
ServiceClient out_of_bounds_client
Service client for determining whether the goal is out of the area bounds.
Definition: position.h:141
bool visualize
Whether to publish the goal waypoint on a topic for visualization.
Definition: position.h:196
#define ROS_ERROR(...)
Duration move_timeout
The time in seconds that reaching a waypoint is allowed to take.
Definition: position.h:191
#define ROS_DEBUG(...)


swarm_behaviors_position
Author(s):
autogenerated on Sat Feb 6 2021 03:11:33