30 #include <visualization_msgs/Marker.h>
31 #include <visualization_msgs/MarkerArray.h>
34 #include <pcl/conversions.h>
35 #include <pcl/point_cloud.h>
36 #include <pcl/point_types.h>
37 #include <interactive_markers/interactive_marker_server.h>
38 #include <interactive_markers/menu_handler.h>
39 #include <boost/lexical_cast.hpp>
40 #include <boost/format.hpp>
41 #include "std_msgs/String.h"
42 #include <mtt/TargetListPC.h>
43 #include <pcl_conversions/pcl_conversions.h>
46 using namespace visualization_msgs;
49 boost::shared_ptr<interactive_markers::InteractiveMarkerServer>
server;
55 void processFeedback(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
57 std::ostringstream mouse_point_ss;
58 if( feedback->mouse_point_valid )
66 switch ( feedback->event_type )
68 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
72 message.header.stamp = ros::Time::now();
73 message.header.frame_id =
"/world";
76 message.id.push_back(0);
79 pcl::PointCloud<pcl::PointXYZ> pc_in;
81 pt.x = feedback->pose.position.x;
82 pt.y = feedback->pose.position.y;
84 pc_in.points.push_back(pt);
85 pcl::toROSMsg(pc_in, message.position);
89 pcl::PointCloud<pcl::PointXYZ> pc_vel;
93 pc_vel.points.push_back(pt);
94 pcl::toROSMsg(pc_vel, message.velocity);
97 pcl::PointCloud<pcl::PointXYZ> pc_obs;
98 pt.x = feedback->pose.position.x +0;
99 pt.y = feedback->pose.position.y -1;
101 pc_obs.points.push_back(pt);
103 pt.x = feedback->pose.position.x +0;
104 pt.y = feedback->pose.position.y +1;
106 pc_obs.points.push_back(pt);
108 pt.x = feedback->pose.position.x +1;
109 pt.y = feedback->pose.position.y +1;
111 pc_obs.points.push_back(pt);
113 sensor_msgs::PointCloud2 pc_msg;
114 pcl::toROSMsg(pc_obs, pc_msg);
116 message.obstacle_lines.push_back(pc_msg);
123 geometry_msgs::Point p;
124 marker.header.frame_id =
"/world";
125 marker.header.stamp = ros::Time::now();
128 marker.action = visualization_msgs::Marker::ADD;
129 marker.type = visualization_msgs::Marker::LINE_LIST;
135 marker.pose.position.x = feedback->pose.position.x;
136 marker.pose.position.y = feedback->pose.position.y;
137 marker.pose.position.z = 0;
142 marker.points.push_back(p);
146 marker.points.push_back(p);
147 marker.points.push_back(p);
151 marker.points.push_back(p);
167 marker.type = Marker::CUBE;
168 marker.scale.x = msg.scale/7;
169 marker.scale.y = msg.scale/7;
170 marker.scale.z = msg.scale/7;
171 marker.color.r = 0.7;
172 marker.color.g = 0.1;
173 marker.color.b = 0.1;
174 marker.color.a = 0.8;
181 InteractiveMarkerControl control;
182 control.always_visible =
true;
183 control.markers.push_back(
makeBox(msg) );
184 msg.controls.push_back( control );
186 return msg.controls.back();
192 InteractiveMarker int_marker;
193 int_marker.header.frame_id =
"/world";
194 int_marker.pose.position.x = 5;
195 int_marker.pose.position.y = 4;
196 int_marker.pose.position.z = 0;
197 int_marker.scale = 0.4;
198 int_marker.name =
"Control obstacle";
199 int_marker.description =
"Control the final \nposition of the obstacle";
203 InteractiveMarkerControl control;
205 control.orientation.w = 1;
206 control.orientation.x = 1;
207 control.orientation.y = 0;
208 control.orientation.z = 0;
209 control.name =
"move_x";
210 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
211 int_marker.controls.push_back(control);
213 control.orientation.w = 1;
214 control.orientation.x = 0;
215 control.orientation.y = 0;
216 control.orientation.z = 1;
217 control.name =
"move_y";
218 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
219 int_marker.controls.push_back(control);
221 server->insert(int_marker);
226 int main(
int argc,
char **argv)
228 ros::init(argc, argv,
"mtt_target_generator");
229 ros::NodeHandle n(
"~");
230 ros::Rate loop_rate(10);
233 coor_pub = n.advertise<mtt::TargetListPC>(
"/mtt_targets", 1000);
235 line_pub = n.advertise<visualization_msgs::Marker>(
"/line_markers", 1 );
238 server.reset(
new interactive_markers::InteractiveMarkerServer(
"mtt_target_generator/im",
"",
false) );
void make6DofMarker(bool fixed)
Marker makeBox(InteractiveMarker &msg)
InteractiveMarkerControl & makeBoxControl(InteractiveMarker &msg)
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
int main(int argc, char **argv)
visualization_msgs::Marker marker
trajectory_planner::coordinates message
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server