36 #include "std_msgs/String.h"
37 #include "geometry_msgs/PoseWithCovariance.h"
38 #include "tf/transform_listener.h"
39 #include "mtt/TargetList.h"
40 #include <visualization_msgs/Marker.h>
46 visualization_msgs::Marker marker;
48 marker.header.frame_id =
"/map";
49 marker.header.stamp = ros::Time::now();
51 marker.ns =
"quality";
52 marker.id = input.orientation.x;
54 marker.type = visualization_msgs::Marker::CYLINDER;
55 marker.action = visualization_msgs::Marker::ADD;
58 marker.pose.position.x = input.position.x;
59 marker.pose.position.y = input.position.y;
60 marker.pose.position.z = 0;
61 marker.pose.orientation.x = 0.0;
62 marker.pose.orientation.y = 0.0;
63 marker.pose.orientation.z = 0.0;
64 marker.pose.orientation.w = 1.0;
72 if(input.position.z == -1){
73 marker.color.r = 0.0f;
74 marker.color.g = 1.0f;
76 else if (input.position.z == 1){
77 marker.color.r = 1.0f;
78 marker.color.g = 0.0f;
81 marker.color.b = 0.2f;
84 marker.lifetime = ros::Duration(2);
90 int main(
int argc,
char **argv)
92 ros::init(argc, argv,
"class_view");
95 ros::Subscriber sub_draw = n.subscribe(
"/pose_to_draw", 1000,
drawCallback);
96 marker_pub = n.advertise<visualization_msgs::Marker>(
"/leader_quality", 1);
void drawCallback(const geometry_msgs::Pose &input)
ros::Publisher marker_pub
Visualization of leader classification.
int main(int argc, char **argv)