40 #include "std_msgs/String.h"
41 #include "geometry_msgs/PoseWithCovariance.h"
42 #include "tf/transform_listener.h"
43 #include "mtt/TargetList.h"
44 #include <visualization_msgs/Marker.h>
45 #include <visualization_msgs/MarkerArray.h>
46 #include "opencv/cv.h"
47 #include "opencv/ml.h"
71 visualization_msgs::MarkerArray
ma;
89 void drawPose(
const geometry_msgs::Pose& input, uint
target,
double classification)
92 visualization_msgs::Marker marker;
94 marker.header.frame_id =
"/map";
95 marker.header.stamp = ros::Time::now();
97 marker.ns =
"quality";
100 marker.type = visualization_msgs::Marker::CYLINDER;
101 marker.action = visualization_msgs::Marker::ADD;
104 marker.pose.position.x = input.position.x;
105 marker.pose.position.y = input.position.y;
106 marker.pose.position.z = 0;
107 marker.pose.orientation.x = 0.0;
108 marker.pose.orientation.y = 0.0;
109 marker.pose.orientation.z = 0.0;
110 marker.pose.orientation.w = 1.0;
113 marker.scale.x = 0.5;
114 marker.scale.y = 0.5;
115 marker.scale.z = 0.5;
118 if(classification == 1){
119 marker.color.r = 0.0f;
120 marker.color.g = 1.0f;
122 else if (classification == 2){
123 marker.color.r = 1.0f;
124 marker.color.g = 0.0f;
127 marker.color.b = 0.0f;
128 marker.color.a = 1.0;
130 marker.lifetime = ros::Duration(1.0);
133 ma.markers.push_back(marker);
141 static ros::Time start_time = ros::Time::now();
153 p_listener->lookupTwist(
"/map",
"/base_link", ros::Time(0), ros::Duration(0.5),
twist);
155 catch (tf::TransformException ex){
156 ROS_ERROR(
"%s",ex.what());
166 for(uint i = 0; i < list.Targets.size(); i++){
168 target_x = list.Targets[i].pose.position.x;
169 target_y = list.Targets[i].pose.position.y;
170 target_theta = tf::getYaw(list.Targets[i].pose.orientation);
171 target_vel = sqrt(pow(list.Targets[i].velocity.linear.x,2)+
172 pow(list.Targets[i].velocity.linear.y,2));
202 int main(
int argc,
char **argv)
204 ros::init(argc, argv,
"process_target");
207 tf::TransformListener listener;
211 std::cout <<
"loaded file: " << argv[1] << std::endl;
219 nfeatures_pub = n.advertise<geometry_msgs::PoseWithCovariance>(
"/example_topic", 1000);
220 marker_pub = n.advertise<visualization_msgs::MarkerArray>(
"/leader_quality", 1000);
visualization_msgs::MarkerArray ma
CvMat * sample_to_classify
ros::Duration duration_btw_msg
void drawPose(const geometry_msgs::Pose &input, uint target, double classification)
std::list< geometry_msgs::PoseWithCovariance > matlab_list
geometry_msgs::Twist features
ros::Publisher nfeatures_pub
tf::TransformListener * p_listener
void targetsCallback(const mtt::TargetList &list)
tf::StampedTransform transform
geometry_msgs::PoseWithCovariance nfeatures
ros::Time time_last_msg(0)
int main(int argc, char **argv)
CvBoost adaboost
Code for feature extraction.
ros::Duration time_elapsed
ros::Publisher marker_pub
geometry_msgs::Twist twist