30 #include <visualization_msgs/Marker.h>
31 #include <visualization_msgs/MarkerArray.h>
33 #include <interactive_markers/interactive_marker_server.h>
34 #include <interactive_markers/menu_handler.h>
35 #include <boost/lexical_cast.hpp>
36 #include <boost/format.hpp>
37 #include "std_msgs/String.h"
38 #include <trajectory_planner/coordinates.h>
41 using namespace visualization_msgs;
44 boost::shared_ptr<interactive_markers::InteractiveMarkerServer>
server;
51 void processFeedback(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
53 if( feedback->mouse_point_valid )
61 switch ( feedback->event_type )
63 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
64 message.x=feedback->pose.position.x;
message.y=feedback->pose.position.y;
message.theta=2.0*asin(feedback->pose.orientation.z);
66 message.header.stamp = ros::Time::now();
67 message.header.frame_id =
"/world";
82 marker.type = Marker::ARROW;
83 marker.scale.x = msg.scale;
84 marker.scale.y = msg.scale;
85 marker.scale.z = msg.scale;
96 InteractiveMarkerControl control;
97 control.always_visible =
true;
98 control.markers.push_back(
makeBox(msg) );
99 msg.controls.push_back( control );
101 return msg.controls.back();
107 InteractiveMarker int_marker;
108 int_marker.header.frame_id =
"/world";
109 int_marker.pose.position.x = 0;
110 int_marker.pose.position.y = 0;
111 int_marker.pose.position.z = 0;
112 int_marker.scale = 0.4;
114 int_marker.name =
"Control target";
115 int_marker.description =
"Control the final \nposition of the robot";
119 InteractiveMarkerControl control;
121 control.orientation.w = 1;
122 control.orientation.x = 1;
123 control.orientation.y = 0;
124 control.orientation.z = 0;
125 control.name =
"move_x";
126 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
127 int_marker.controls.push_back(control);
129 control.orientation.w = 1;
130 control.orientation.x = 0;
131 control.orientation.y = 0;
132 control.orientation.z = 1;
133 control.name =
"move_y";
134 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
135 int_marker.controls.push_back(control);
137 control.orientation.w = 1;
138 control.orientation.x = 0;
139 control.orientation.y = 1;
140 control.orientation.z = 0;
141 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
142 control.name =
"rotate_z";
143 int_marker.controls.push_back(control);
145 server->insert(int_marker);
150 int main(
int argc,
char **argv)
152 ros::init(argc, argv,
"APgenerator");
153 ros::NodeHandle n(
"~");
154 ros::Rate loop_rate(10);
157 coor_pub = n.advertise<trajectory_planner::coordinates>(
"/msg_coordinates", 1000);
160 server.reset(
new interactive_markers::InteractiveMarkerServer(
"APgenerator/im",
"",
false) );
Marker makeBox(InteractiveMarker &msg)
void make6DofMarker(bool fixed)
InteractiveMarkerControl & makeBoxControl(InteractiveMarker &msg)
int main(int argc, char **argv)
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
visualization_msgs::Marker marker
trajectory_planner::coordinates message
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server