9 visualization_msgs::Marker
marker;
10 marker.type = visualization_msgs::Marker::CUBE;
21 visualization_msgs::Marker
marker;
22 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
23 marker.text =
"hello world";
35 visualization_msgs::InteractiveMarker mk;
36 mk.header.frame_id = base_frame;
37 if (latest_feedback_) {
38 mk.pose = latest_feedback_->pose;
41 std::string description;
43 ss <<
"size: (" << mconfig.
size.x
44 <<
", " << mconfig.
size.y
45 <<
", " << mconfig.
size.z <<
")" << std::endl;
46 ss <<
"resolution: " << mconfig.
resolution_ << std::endl;
47 description = ss.str();
56 visualization_msgs::InteractiveMarkerControl controlBox;
57 controlBox.always_visible =
true;
58 controlBox.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
59 controlBox.markers.push_back( makeBoxMarker(mconfig.
size) );
60 mk.controls.push_back( controlBox );
62 visualization_msgs::InteractiveMarkerControl textBox;
63 textBox.always_visible =
true;
64 textBox.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
65 visualization_msgs::Marker text_marker = makeTextMarker(mconfig.
size);
66 text_marker.text = description;
67 textBox.markers.push_back( text_marker );
68 mk.controls.push_back( textBox );
70 visualization_msgs::InteractiveMarkerControl controlArrow;
71 controlArrow.always_visible =
true;
72 controlArrow.orientation.w = 1;
73 controlArrow.orientation.x = 1;
74 controlArrow.orientation.y = 0;
75 controlArrow.orientation.z = 0;
77 controlArrow.name =
"move_x";
78 controlArrow.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
79 mk.controls.push_back(controlArrow);
81 controlArrow.orientation.w = 1;
82 controlArrow.orientation.x = 0;
83 controlArrow.orientation.y = 1;
84 controlArrow.orientation.z = 0;
85 controlArrow.name =
"move_z";
86 controlArrow.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
87 mk.controls.push_back(controlArrow);
89 controlArrow.orientation.w = 1;
90 controlArrow.orientation.x = 0;
91 controlArrow.orientation.y = 0;
92 controlArrow.orientation.z = 1;
93 controlArrow.name =
"move_y";
94 controlArrow.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
95 mk.controls.push_back(controlArrow);
110 resolution_menu_ = mh.
insert(
"Resolution" );
121 marker_control_config.resolution_ = 0.05;
122 checked_resolution_menu_ = resolution_5cm_menu_;
124 box_size_menu_ = mh.
insert(
"Box Size" );
133 checked_box_size_menu_ = box_size_50_menu_;
150 current_pose_pub_.publish(pose);
155 geometry_msgs::PoseStamped pose_stamped;
156 pose_stamped.header = feedback->header;
157 pose_stamped.pose = feedback->pose;
158 current_pose_pub_.publish(pose_stamped);
164 publishCurrentPose(feedback);
165 latest_feedback_ = feedback;
170 checked_resolution_menu_ = feedback->menu_entry_id;
172 if(feedback->menu_entry_id == resolution_10cm_menu_){
173 marker_control_config.resolution_ = 0.1;
175 }
else if(feedback->menu_entry_id == resolution_5cm_menu_){
176 marker_control_config.resolution_ = 0.05;
178 else if(feedback->menu_entry_id == resolution_20cm_menu_){
179 marker_control_config.resolution_ = 0.2;
181 std::cout <<
"resolution:" << marker_control_config.resolution_ << std::endl;
185 menu_handler.reApply( *server_ );
186 server_->applyChanges();
187 latest_feedback_ = feedback;
193 marker_control_config.resolution_ = msg->data;
194 updateBoxInteractiveMarker();
198 marker_control_config.size = size;
199 updateBoxInteractiveMarker();
205 checked_resolution_menu_ = feedback->menu_entry_id;
209 geometry_msgs::Vector3 vec;
210 if(feedback->menu_entry_id == box_size_100_menu_){
215 }
else if(feedback->menu_entry_id == box_size_50_menu_){
224 visualization_msgs::Marker
marker;
225 marker.type = visualization_msgs::Marker::CUBE;
226 marker.pose = feedback->pose;
227 marker.scale.x = marker_control_config.size.x;
228 marker.scale.y = marker_control_config.size.y;
229 marker.scale.z = marker_control_config.size.z;
230 marker.color.r = marker_control_config.resolution_;
231 marker.color.g = marker_control_config.resolution_ * 10;
232 if(marker.color.g > 1.0){marker.color.g = 1.0;}
233 marker.color.b = marker_control_config.resolution_ * 10;
234 if(marker.color.b > 1.0){marker.color.b = 1.0;}
235 marker.color.a = 0.1;
236 marker.header = feedback->header;
241 visualization_msgs::Marker
marker = makeMarkerMsg(feedback);
242 marker.id = marker_control_config.marker_id++;
243 pub_.publish(marker);
244 latest_feedback_ = feedback;
249 if(marker_control_config.marker_id == 0){
252 visualization_msgs::Marker
marker = makeMarkerMsg(feedback);
253 marker.id = --marker_control_config.marker_id;
254 marker.action = visualization_msgs::Marker::DELETE;
255 pub_.publish(marker);
256 latest_feedback_ = feedback;
261 marker_control_config.marker_id = 0;
263 visualization_msgs::Marker
marker = makeMarkerMsg(latest_feedback_);
265 marker.action = visualization_msgs::Marker::DELETE;
266 pub_.publish(marker);
274 latest_feedback_ = feedback;
279 visualization_msgs::InteractiveMarker boxIM = makeBoxInteractiveMarker(marker_control_config, marker_name);
281 server_->insert(boxIM,
283 menu_handler.apply(*server_, marker_name);
284 server_->applyChanges();
289 server_->setPose(marker_name, pose->pose);
290 server_->applyChanges();
291 publishCurrentPose(pose);
293 visualization_msgs::InteractiveMarkerFeedback::Ptr feedback(
new visualization_msgs::InteractiveMarkerFeedback);
294 feedback->pose = pose->pose;
295 feedback->header = pose->header;
296 latest_feedback_ = feedback;
301 visualization_msgs::Marker
marker = makeMarkerMsg(latest_feedback_);
302 marker.id = marker_control_config.marker_id++;
303 pub_.publish(marker);
348 int main(
int argc,
char** argv)
350 ros::init(argc, argv,
"point_cloud_config_marker");
ros::Subscriber pose_update_sub_
visualization_msgs::Marker makeBoxMarker(geometry_msgs::Vector3 size)
MarkerControlConfig marker_control_config
ros::Publisher current_pose_pub_
void updatePoseCB(const geometry_msgs::PoseStamped::ConstPtr &pose)
interactive_markers::MenuHandler makeMenuHandler()
visualization_msgs::InteractiveMarker makeBoxInteractiveMarker(MarkerControlConfig mconfig, std::string name)
void clearCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void changeBoxSizeCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void changeBoxResolution(const std_msgs::Float32::ConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
void moveBoxCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
ros::Subscriber change_box_resolution_sub_
visualization_msgs::Marker makeMarkerMsg(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
std::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
void publishMarkerMsg(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
ROSCPP_DECL void spin(Spinner &spinner)
void addBoxCB(const std_msgs::Empty::ConstPtr &msg)
bool setCheckState(EntryHandle handle, CheckState check_state)
void updateBoxInteractiveMarker()
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishCurrentPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
ros::Subscriber change_box_size_sub_
void clearBoxCB(const std_msgs::Empty::ConstPtr &msg)
EntryHandle insert(const std::string &title, const FeedbackCallback &feedback_cb)
ros::Subscriber add_box_sub_
geometry_msgs::Vector3 size
void cancelCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
visualization_msgs::Marker makeTextMarker(geometry_msgs::Vector3 size)
double max(double a, double b)
ros::Subscriber clear_box_sub_
void changeBoxSize(geometry_msgs::Vector3 size)
interactive_markers::MenuHandler menu_handler
void changeResolutionCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)