target_monitor.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <tf2/utils.h>
3 #include <actionlib/server/simple_action_server.h>
4 #include <geometry_msgs/Pose.h>
5 #include <cpswarm_msgs/TaskAllocatedEvent.h>
6 #include <cpswarm_msgs/TargetPositionEvent.h>
7 #include <cpswarm_msgs/TargetTracking.h>
8 #include <cpswarm_msgs/TargetAction.h>
9 #include "lib/targets.h"
10 
11 using namespace std;
12 using namespace ros;
13 
17 typedef actionlib::SimpleActionServer<cpswarm_msgs::TargetAction> action_server_t;
18 
23 
27 geometry_msgs::Pose pose;
28 
33 
38 double yaw ()
39 {
40  tf2::Quaternion orientation;
41  tf2::fromMsg(pose.orientation, orientation);
42  return tf2::getYaw(orientation);
43 }
44 
49 geometry_msgs::Quaternion rotate (geometry_msgs::Quaternion rotation)
50 {
51  tf2::Quaternion cps_orientation;
52  tf2::fromMsg(pose.orientation, cps_orientation);
53  tf2::Quaternion target_rotation;
54  tf2::fromMsg(rotation, target_rotation);
55  tf2::Quaternion target_orientation = target_rotation * cps_orientation;
56  target_orientation.normalize();
57  return tf2::toMsg(target_orientation);
58 }
59 
64 void tracking_callback (const cpswarm_msgs::TargetTracking::ConstPtr& msg)
65 {
66  // compute distance and direction of target
67  double distance = hypot(msg->tf.translation.x, msg->tf.translation.y);
68  double direction = yaw() + atan2(msg->tf.translation.y, -msg->tf.translation.x) - M_PI / 2; // x is inverted in tracking camera tf
69 
70  // create position event message to update target internally
71  cpswarm_msgs::TargetPositionEvent event;
72  event.id = msg->id;
73  event.pose.pose.position.x = pose.position.x + distance * cos(direction);
74  event.pose.pose.position.y = pose.position.y + distance * sin(direction);
75  event.pose.pose.orientation = rotate(msg->tf.rotation);
76  event.header.stamp = msg->header.stamp;
77  monitor->update(event, TARGET_TRACKED);
78 }
79 
84 void found_callback (const cpswarm_msgs::TargetPositionEvent::ConstPtr& msg)
85 {
86  monitor->update(*msg, TARGET_KNOWN);
87 }
88 
93 void update_callback (const cpswarm_msgs::TargetPositionEvent::ConstPtr& msg)
94 {
95  monitor->update(*msg, TARGET_KNOWN);
96 }
97 
102 void assigned_callback (const cpswarm_msgs::TaskAllocatedEvent::ConstPtr& msg)
103 {
104  // create position event message to update target internally
105  cpswarm_msgs::TargetPositionEvent event;
106  event.id = msg->task_id;
107  event.header.stamp = msg->header.stamp;
108  monitor->update(event, TARGET_ASSIGNED);
109 }
110 
115 void lost_callback (const cpswarm_msgs::TargetPositionEvent::ConstPtr& msg)
116 {
117  monitor->update(*msg, TARGET_LOST);
118 }
119 
124 void done_callback (const cpswarm_msgs::TargetPositionEvent::ConstPtr& msg)
125 {
126  monitor->update(*msg, TARGET_DONE);
127 }
128 
133 void pose_callback (const geometry_msgs::PoseStamped::ConstPtr& msg)
134 {
135  // valid pose received
136  if (msg->header.stamp.isValid())
137  pose_valid = true;
138 
139  // store new position and orientation in class variables
140  pose = msg->pose;
141 
142  ROS_DEBUG_THROTTLE(1, "Yaw %.2f", yaw());
143  ROS_DEBUG_THROTTLE(1, "Pose [%.2f, %.2f, %.2f]", pose.position.x, pose.position.y, pose.position.z);
144 }
145 
151 void set_done(const cpswarm_msgs::TargetGoalConstPtr& goal, action_server_t* as)
152 {
153  // create position event message
154  cpswarm_msgs::TargetPositionEvent event;
155  event.id = goal->id;
156  event.pose = goal->pose;
157  event.header = goal->pose.header;
158 
159  // update target internally
160  monitor->update(event, TARGET_DONE);
161 
162  // action server finished successfully
163  as->setSucceeded();
164 }
165 
172 int main (int argc, char** argv)
173 {
174  // init ros node
175  init(argc, argv, "target_monitor");
176  NodeHandle nh;
177 
178  // read parameters
179  bool simulation;
180  nh.param(this_node::getName() + "/simulation", simulation, false);
181  int queue_size;
182  nh.param(this_node::getName() + "/queue_size", queue_size, 10);
183 
184  // rate of update loop
185  double loop_rate;
186  nh.param(this_node::getName() + "/loop_rate", loop_rate, 5.0);
187  Rate rate(loop_rate);
188 
189  // create target monitor
190  monitor = new targets();
191 
192  // read target positions from parameter file
193  if (simulation) {
194  monitor->simulate();
195  }
196 
197  // no pose received yet
198  pose_valid = false;
199 
200  // publishers and subscribers
201  Subscriber pose_sub = nh.subscribe("pos_provider/pose", queue_size, pose_callback);
202  Subscriber tracking_sub = nh.subscribe("target_tracking", queue_size, tracking_callback);
203  Subscriber local_assigned_sub = nh.subscribe("cps_selected", queue_size, assigned_callback);
204  Subscriber local_done_sub = nh.subscribe("target_done", queue_size, done_callback);
205  Subscriber found_sub = nh.subscribe("bridge/events/target_found", queue_size, found_callback);
206  Subscriber update_sub = nh.subscribe("bridge/events/target_update", queue_size, update_callback);
207  Subscriber assigned_sub = nh.subscribe("bridge/events/cps_selected", queue_size, assigned_callback);
208  Subscriber lost_sub = nh.subscribe("bridge/events/target_lost", queue_size, lost_callback);
209  Subscriber done_sub = nh.subscribe("bridge/events/target_done", queue_size, done_callback);
210 
211  // action servers
212  action_server_t set_done_as(nh, "cmd/target_done", boost::bind(&set_done, _1, &set_done_as), false);
213  set_done_as.start();
214 
215  // init position and yaw
216  while (ok() && pose_valid == false) {
217  ROS_DEBUG_ONCE("Waiting for valid pose...");
218  rate.sleep();
219  spinOnce();
220  }
221 
222  // update information about targets
223  while (ok()) {
224  monitor->update(pose);
225  rate.sleep();
226  spinOnce();
227  }
228 
229  // destroy target monitor
230  delete monitor;
231 
232  return 0;
233 }
void found_callback(const cpswarm_msgs::TargetPositionEvent::ConstPtr &msg)
Callback function to receive information about targets found by other CPSs.
targets * monitor
The targets being monitored.
void lost_callback(const cpswarm_msgs::TargetPositionEvent::ConstPtr &msg)
Callback function to receive information about targets lost by other CPSs.
int main(int argc, char **argv)
Main function to be executed by ROS.
void simulate()
Read simulated targets from parameter file.
Definition: targets.cpp:34
void update_callback(const cpswarm_msgs::TargetPositionEvent::ConstPtr &msg)
Callback function to receive updated information about targets from other CPSs.
void assigned_callback(const cpswarm_msgs::TaskAllocatedEvent::ConstPtr &msg)
Callback function to receive information about targets assigned to a CPSs.
void update(geometry_msgs::Pose pose)
Update the state of all targets. If no update has been received for a target within a fixed period...
Definition: targets.cpp:57
double yaw()
Compute the current yaw orientation of the CPS.
void tracking_callback(const cpswarm_msgs::TargetTracking::ConstPtr &msg)
Callback function for target position.
void set_done(const cpswarm_msgs::TargetGoalConstPtr &goal, action_server_t *as)
Callback of the action server which sets the target state to done.
void done_callback(const cpswarm_msgs::TargetPositionEvent::ConstPtr &msg)
Callback function for incoming target done events.
geometry_msgs::Quaternion rotate(geometry_msgs::Quaternion rotation)
Compute the orientation resulting from the rotated CPS orientation.
void pose_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
Callback function for position updates.
A collection of target objects that are searched, tracked, and rescued by the CPSs.
Definition: targets.h:15
bool pose_valid
Whether a valid position has been received.
geometry_msgs::Pose pose
Current position of the CPS.
actionlib::SimpleActionServer< cpswarm_msgs::TargetAction > action_server_t
An action server type that allows to set the state of a target.


target_monitor
Author(s): Micha Sende
autogenerated on Thu Oct 31 2019 15:47:40