7 nh.
param(this_node::getName() +
"/loop_rate", loop_rate, 5.0);
10 nh.
param(this_node::getName() +
"/queue_size", queue_size, 1);
12 double d_move_timeout;
13 nh.
param(this_node::getName() +
"/move_timeout", d_move_timeout, 10.0);
26 pose_pub =
nh.
advertise<geometry_msgs::PoseStamped>(
"pos_controller/goal_position", queue_size,
true);
48 double b = remainder(atan2(p.position.y -
pose.position.y, p.position.x -
pose.position.x) -
get_yaw(), 2*M_PI);
61 geometry_msgs::Pose
goal;
64 goal.position.x = start.position.x + distance * cos(direction);
65 goal.position.y = start.position.y + distance * sin(direction);
70 orientation.
setRPY(0, 0, direction);
83 return hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y);
105 ROS_ERROR(
"Cannot move to (%.2f,%.2f) because it is out of bounds!", goal.position.x, goal.position.y);
112 ROS_ERROR(
"Obstacle ahead, stop moving!");
118 if (this->goal.header.stamp.isZero() || this->goal.pose.position.x != goal.position.x || this->goal.pose.position.y != goal.position.y) {
121 geometry_msgs::PointStamped wp;
122 wp.header.stamp = Time::now();
123 wp.header.frame_id =
"local_origin_ned";
124 wp.point = goal.position;
129 this->goal.header.stamp = Time::now();
130 this->goal.pose =
goal;
131 this->goal.pose.position.z =
altitude;
144 cpswarm_msgs::GetSector ocs;
147 if (ocs.response.min == ocs.response.max)
150 ROS_DEBUG(
"Goal %.2f in occupied sector [%.2f,%.2f]?",
bearing(pose), ocs.response.min, ocs.response.max);
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;
156 return ocs.response.min <=
bearing(pose) &&
bearing(pose) <= ocs.response.max;
159 ROS_ERROR(
"Failed to check if goal is occupied");
166 cpswarm_msgs::OutOfBounds oob;
167 oob.request.pose =
pose;
169 return oob.response.out;
172 ROS_ERROR(
"Failed to check if goal is out of bounds");
180 ROS_DEBUG(
"Pose (%.2f,%.2f) --> (%.2f,%.2f)",
pose.position.x,
pose.position.y,
goal.pose.position.x,
goal.pose.position.y);
184 if (
goal.header.stamp.isZero())
205 goal.header.stamp = Time::now();
211 ROS_INFO(
"Stopping in [%.2f, %.2f]",
goal.pose.position.x,
goal.pose.position.y);
227 if (msg->header.stamp.isValid())
double bearing(geometry_msgs::Pose p) const
Compute the bearing from the current pose of the CPS to a given pose.
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.
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
double altitude
The altitude at which the CPS operates.
bool move(geometry_msgs::Pose pose)
Move the CPS to the given pose.
NodeHandle nh
A node handle for the main ROS node.
position(double altitude)
Constructor that initializes the private member variables.
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.
~position()
Destructor that deletes the private member objects.
#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.
geometry_msgs::PoseStamped goal
Current goal of the CPS.
bool reached()
Check whether the CPS has reached the current goal.
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.
geometry_msgs::Pose pose
Current position of the CPS.
double goal_tolerance
The distance that the CPS can be away from a goal while still being considered to have reached that g...
void pose_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
Callback function for position updates.
double get_yaw() const
Get the current yaw orientation of the CPS.
Publisher pose_pub
Publisher for sending the goal position of the CPS to the position controller in the abstraction libr...
geometry_msgs::Pose get_pose() const
Get the current pose of the CPS.
double get_tolerance() const
Get the position tolerance.
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void fromMsg(const A &, B &b)
ServiceClient occupied_sector_client
Service client for determining the sector occupied by obstacles.
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.
double getYaw(const A &a)
bool out_of_bounds(geometry_msgs::Pose pose)
Check whether a given pose is out of the mission area boundaries.
void stop()
Stop moving by publishing the current position as goal.
Subscriber pose_sub
Subscriber for the position of the CPS.
double dist(geometry_msgs::Pose p) const
Compute the straight-line distance from the current position to the given position.
ROSCPP_DECL void spinOnce()
ServiceClient out_of_bounds_client
Service client for determining whether the goal is out of the area bounds.
bool visualize
Whether to publish the goal waypoint on a topic for visualization.
Duration move_timeout
The time in seconds that reaching a waypoint is allowed to take.