27 #ifndef _trajectory_planner_nodelet_aux_CPP_
28 #define _trajectory_planner_nodelet_aux_CPP_
32 void processFeedback(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
35 s <<
"Feedback from marker '" << feedback->marker_name <<
"' "
36 <<
" / control '" << feedback->control_name <<
"'";
38 std::ostringstream mouse_point_ss;
39 if( feedback->mouse_point_valid )
41 mouse_point_ss <<
" at " << feedback->mouse_point.x
42 <<
", " << feedback->mouse_point.y
43 <<
", " << feedback->mouse_point.z
44 <<
" in frame " << feedback->header.frame_id;
47 switch ( feedback->event_type )
49 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
50 manage_vt->set_attractor_point(feedback->pose.position.x, feedback->pose.position.y, 2.0*asin(feedback->pose.orientation.z));
63 marker.type = Marker::ARROW;
64 marker.scale.x = msg.scale;
65 marker.scale.y = msg.scale;
66 marker.scale.z = msg.scale;
77 InteractiveMarkerControl control;
78 control.always_visible =
true;
79 control.markers.push_back(
makeBox(msg) );
80 msg.controls.push_back( control );
82 return msg.controls.back();
88 InteractiveMarker int_marker;
89 int_marker.header.frame_id =
"/vehicle_odometry";
90 int_marker.pose.position.x = 5;
91 int_marker.pose.position.y = 0;
92 int_marker.pose.position.z = 0;
93 int_marker.scale = 0.4;
95 int_marker.name =
"Control target";
96 int_marker.description =
"Control the final \nposition of the robot";
100 InteractiveMarkerControl control;
102 control.orientation.w = 1;
103 control.orientation.x = 1;
104 control.orientation.y = 0;
105 control.orientation.z = 0;
106 control.name =
"move_x";
107 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
108 int_marker.controls.push_back(control);
110 control.orientation.w = 1;
111 control.orientation.x = 0;
112 control.orientation.y = 0;
113 control.orientation.z = 1;
114 control.name =
"move_y";
115 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
116 int_marker.controls.push_back(control);
118 control.orientation.w = 1;
119 control.orientation.x = 0;
120 control.orientation.y = 1;
121 control.orientation.z = 0;
122 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
123 control.name =
"rotate_z";
124 int_marker.controls.push_back(control);
126 server->insert(int_marker);
_EXTERN_ c_manage_trajectoryPtr manage_vt
Marker makeBox(InteractiveMarker &msg)
void make6DofMarker(bool fixed)
InteractiveMarkerControl & makeBoxControl(InteractiveMarker &msg)
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
visualization_msgs::Marker marker
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server