2 #include <visualization_msgs/Marker.h>
3 #include <visualization_msgs/MarkerArray.h>
4 #include "mtt/TargetList.h"
5 #include "sensor_msgs/LaserScan.h"
9 #include <boost/thread.hpp>
10 #include <boost/shared_ptr.hpp>
37 void update(visualization_msgs::Marker& marker)
39 for(uint i=0;i<
markers.size();++i)
54 for(uint i=0;i<
markers.size();++i)
58 case visualization_msgs::Marker::ADD:
59 markers[i].action = visualization_msgs::Marker::DELETE;
61 case visualization_msgs::Marker::DELETE:
74 vector<visualization_msgs::Marker> new_markers;
76 for(uint i=0;i<
markers.size();++i)
78 new_markers.push_back(
markers[i]);
90 vector<visualization_msgs::Marker> m_out(
markers);
95 vector<visualization_msgs::Marker>
markers;
108 typedef boost::shared_ptr<Candidate>
Ptr;
114 priority = priority_;
127 switch(classification)
142 cout<<
"Processing image"<<endl;
146 boost::this_thread::sleep(boost::posix_time::milliseconds(100));
170 vector<Candidate::Ptr> candidate_list;
173 visualization_msgs::MarkerArray allmarkers;
180 for(
int i=0; i<msg.Targets.size();i++)
183 if (msg.Targets[i].size > 0.01 )
191 candidate->distance_to_target_from_center_bumper = sqrt(msg.Targets[i].pose.position.x * msg.Targets[i].pose.position.x +
192 msg.Targets[i].pose.position.y * msg.Targets[i].pose.position.y);
196 candidate->theta = acos( msg.Targets[i].pose.position.x / candidate->distance_to_target_from_center_bumper);
200 if(msg.Targets[i].velocity.linear.x > 0.01 )
202 visualization_msgs::Marker marker;
204 marker.header.frame_id =
"laser_1";
205 marker.header.stamp = ros::Time();
206 marker.ns =
"my_namespace";
207 marker.id = msg.Targets[i].id;
208 marker.type = visualization_msgs::Marker::CUBE;
209 marker.action = visualization_msgs::Marker::ADD;
210 marker.pose.position.x = msg.Targets[i].pose.position.x;
211 marker.pose.position.y = msg.Targets[i].pose.position.y;
212 marker.pose.position.z = msg.Targets[i].pose.position.z;
213 marker.pose.orientation.x = 0.0;
214 marker.pose.orientation.y = 0.0;
215 marker.pose.orientation.z = 0.0;
216 marker.pose.orientation.w = 1.0;
217 marker.scale.x = 0.1;
218 marker.scale.y = msg.Targets[i].size * exp(-candidate->distance_to_target_from_center_bumper /100) * 2;
219 marker.scale.z = exp(-(candidate->distance_to_target_from_center_bumper /100)*2);
220 marker.color.a = 1.0;
221 marker.color.r = 0.0;
222 marker.color.g = 1.0;
223 marker.color.b = 0.0;
226 candidate_list.push_back(candidate);
234 sort(candidate_list.begin(), candidate_list.end(),
236 return c1->distance_to_target_from_center_bumper > c2->distance_to_target_from_center_bumper;
253 int main (
int argc,
char **argv)
255 ros::init(argc, argv,
"laser_gather");
257 ros::NodeHandle nh(
"~");
259 marker_publisher = nh.advertise<visualization_msgs::MarkerArray>(
"/ped_markers", 10);
260 ros::Subscriber sub = nh.subscribe (
"/targets", 1000,
laserGather);
ros::Publisher marker_publisher
double distance_to_target_from_center_bumper
Dynamic markers support class.
void laserGather(const mtt::TargetList &msg)
vector< visualization_msgs::Marker > markers
Internal storing vector of markers.
void update(visualization_msgs::Marker &marker)
Update a internal marker.
Candidate(double input_image, uint priority_, uint id_)
void decrement(void)
Mark existing markers for deletion.
string getClassification()
vector< visualization_msgs::Marker > getOutgoingMarkers(void)
Obtain the list of outgoing markers.
void clean(void)
Remove markers that should not be transmitted.
boost::shared_ptr< Candidate > Ptr
int main(int argc, char **argv)