position.cpp
Go to the documentation of this file.
1 #include "position.h"
2 
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_timeout", goal_timeout, 30.0);
12  nh.param(this_node::getName() + "/goal_tolerance", goal_tolerance, 0.1);
13 
14  // no pose received yet
15  pose_valid = false;
16 
17  // init ros communication
18  out_of_bounds_client = nh.serviceClient<cpswarm_msgs::OutOfBounds>("area/out_of_bounds");
19  out_of_bounds_client.waitForExistence();
20  occupied_sector_client = nh.serviceClient<cpswarm_msgs::GetSector>("obstacle_detection/get_occupied_sector");
21  occupied_sector_client.waitForExistence();
22  pose_sub = nh.subscribe("pos_provider/pose", queue_size, &position::pose_callback, this);
23  pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pos_controller/goal_position", queue_size, true);
24 
25  // init position and yaw
26  while (ok() && pose_valid == false) {
27  ROS_DEBUG_ONCE("Waiting for valid pose...");
28  rate->sleep();
29  spinOnce();
30  }
31 }
32 
34 {
35  delete rate;
36 }
37 
38 double position::bearing (geometry_msgs::Pose p) const
39 {
40  double b = remainder(atan2(p.position.y - pose.position.y, p.position.x - pose.position.x) - get_yaw(), 2*M_PI);
41  if (b < 0)
42  b += 2*M_PI;
43  return b;
44 }
45 
46 geometry_msgs::Pose position::compute_goal (double distance, double direction) const
47 {
48  return compute_goal(pose, distance, direction);
49 }
50 
51 geometry_msgs::Pose position::compute_goal (geometry_msgs::Pose start, double distance, double direction) const
52 {
53  geometry_msgs::Pose goal;
54 
55  // calculate position
56  goal.position.x = start.position.x + distance * cos(direction);
57  goal.position.y = start.position.y + distance * sin(direction);
58  goal.position.z = start.position.z;
59 
60  // calculate orientation
61  tf2::Quaternion orientation;
62  orientation.setRPY(0, 0, direction);
63  goal.orientation = tf2::toMsg(orientation);
64 
65  return goal;
66 }
67 
68 double position::dist (geometry_msgs::Pose p) const
69 {
70  return dist(pose, p);
71 }
72 
73 double position::dist (geometry_msgs::Pose p1, geometry_msgs::Pose p2) const
74 {
75  return hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y);
76 }
77 
78 geometry_msgs::Pose position::get_pose () const
79 {
80  return pose;
81 }
82 
83 double position::get_yaw () const
84 {
85  return get_yaw(pose);
86 }
87 
88 bool position::move (geometry_msgs::Pose goal)
89 {
90  // goal is out of bounds
91  if (out_of_bounds(goal)) {
92  ROS_ERROR("Cannot move to (%.2f,%.2f) because it is out of bounds!", goal.position.x, goal.position.y);
93  return false;
94  }
95 
96  // create goal pose
97  geometry_msgs::PoseStamped goal_pose;
98  goal_pose.header.stamp = Time::now();
99  goal_pose.pose = goal;
100 
101  // send goal pose to cps controller
102  pose_pub.publish(goal_pose);
103 
104  ROS_INFO("Move to (%.2f,%.2f)", goal.position.x, goal.position.y);
105 
106  // wait until cps reached goal
107  Time start = Time::now();
108  while (ok() && reached(goal) == false && Time::now() <= start + Duration(goal_timeout)) {
109  // wait
110  rate->sleep();
111 
112  // check if reached goal
113  spinOnce();
114  }
115 
116  // could not reach goal within time
117  if (Time::now() > start + Duration(goal_timeout)) {
118  ROS_WARN("Failed to reach goal (%.2f,%.2f) in timeout %.2fs!", goal.position.x, goal.position.y, goal_timeout);
119  }
120 
121  // successfully moved to goal
122  return true;
123 }
124 
125 bool position::occupied (geometry_msgs::Pose pose)
126 {
127  // get occupied sector
128  cpswarm_msgs::GetSector ocs;
129  if (occupied_sector_client.call(ocs)) {
130  // no obstacles
131  if (ocs.response.min == ocs.response.max)
132  return false;
133 
134  ROS_ERROR("Goal %.2f in occupied sector [%.2f,%.2f]?", bearing(pose), ocs.response.min, ocs.response.max);
135 
136  // check if pose is in this sector
137  if (ocs.response.max > 2*M_PI && bearing(pose) < M_PI)
138  return ocs.response.min <= bearing(pose) + 2*M_PI && bearing(pose) + 2*M_PI <= ocs.response.max; // always max > min
139  else
140  return ocs.response.min <= bearing(pose) && bearing(pose) <= ocs.response.max; // always max > min
141  }
142  else {
143  ROS_ERROR("Failed to check if goal is occupied");
144  return true;
145  }
146 }
147 
148 bool position::out_of_bounds (geometry_msgs::Pose pose)
149 {
150  cpswarm_msgs::OutOfBounds oob;
151  oob.request.pose = pose;
152  if (out_of_bounds_client.call(oob)) {
153  return oob.response.out;
154  }
155  else {
156  ROS_ERROR("Failed to check if goal is out of bounds");
157  return true;
158  }
159 }
160 
161 double position::get_yaw (geometry_msgs::Pose pose) const
162 {
163  tf2::Quaternion orientation;
164  tf2::fromMsg(pose.orientation, orientation);
165  return tf2::getYaw(orientation);
166 }
167 
168 bool position::reached (geometry_msgs::Pose goal)
169 {
170  ROS_DEBUG("Yaw %.2f --> %.2f", get_yaw(pose), get_yaw(goal));
171  ROS_DEBUG("Pose (%.2f,%.2f) --> (%.2f,%.2f)", pose.position.x, pose.position.y, goal.position.x, goal.position.y);
172  ROS_DEBUG("%.2f > %.2f", dist(pose, goal), goal_tolerance);
173 
174  // whether cps reached goal position, ignoring yaw
175  return dist(pose, goal) <= goal_tolerance;
176 }
177 
178 void position::pose_callback (const geometry_msgs::PoseStamped::ConstPtr& msg)
179 {
180  // valid pose received
181  if (msg->header.stamp.isValid())
182  pose_valid = true;
183 
184  // store new position and orientation in class variables
185  pose = msg->pose;
186 
187  ROS_DEBUG_THROTTLE(1, "Yaw %.2f", get_yaw());
188  ROS_DEBUG_THROTTLE(1, "Pose [%.2f, %.2f, %.2f]", pose.position.x, pose.position.y, pose.position.z);
189 }
double goal_timeout
The time in seconds that the CPS is given time to reach a destination before giving up...
Definition: position.h:165
bool move(geometry_msgs::Pose pose)
Move the CPS to the given pose.
Definition: position.cpp:88
NodeHandle nh
A node handle for the main ROS node.
Definition: position.h:145
double dist(geometry_msgs::Pose p) const
Compute the straight-line distance from the current position to the given position.
Definition: position.cpp:68
Rate * rate
The loop rate object for running the behavior control loops at a specific frequency.
Definition: position.h:150
~position()
Destructor that deletes the private member objects.
Definition: position.cpp:33
bool pose_valid
Whether a valid position has been received.
Definition: position.h:160
double bearing(geometry_msgs::Pose p) const
Compute the bearing from the current pose of the CPS to a given pose.
Definition: position.cpp:38
position()
Constructor that initializes the private member variables.
Definition: position.cpp:3
bool occupied(geometry_msgs::Pose pose)
Check whether there is an obstacle in the direction of the given pose.
Definition: position.cpp:125
geometry_msgs::Pose pose
Current position of the CPS.
Definition: position.h:155
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:170
void pose_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
Callback function for position updates.
Definition: position.cpp:178
geometry_msgs::Pose compute_goal(double distance, double direction) const
Compute the goal coordinates relative to the current position.
Definition: position.cpp:46
Publisher pose_pub
Publisher for sending the goal position of the CPS to the position controller in the abstraction libr...
Definition: position.h:140
ServiceClient occupied_sector_client
Service client for determining the sector occupied by obstacles.
Definition: position.h:135
bool out_of_bounds(geometry_msgs::Pose pose)
Check whether a given pose is out of the mission area boundaries.
Definition: position.cpp:148
Subscriber pose_sub
Subscriber for the position of the CPS.
Definition: position.h:125
bool reached(geometry_msgs::Pose goal)
Check whether the CPS has reached a given pose.
Definition: position.cpp:168
double get_yaw() const
Get the current yaw orientation of the CPS.
Definition: position.cpp:83
ServiceClient out_of_bounds_client
Service client for determining whether the goal is out of the area bounds.
Definition: position.h:130
geometry_msgs::Pose get_pose() const
Get the current pose of the CPS.
Definition: position.cpp:78


position
Author(s):
autogenerated on Sun Dec 29 2019 10:25:17