52 config_srv_ = std::make_shared <dynamic_reconfigure::Server<InteractiveSettingConfig> > (*n_);
53 dynamic_reconfigure::Server<InteractiveSettingConfig>::CallbackType
f =
60 std::string yaml_filename;
61 n_->param(
"yaml_filename", yaml_filename, std::string(
""));
67 "disable manipulator",
70 bool use_parent_and_child;
71 n_->param(
"use_parent_and_child", use_parent_and_child,
false);
72 if (use_parent_and_child)
74 ROS_INFO(
"initialize parent and child marker");
79 ROS_INFO(
"initialize simple marker");
87 delete itpairstri->second;
96 int interaction_mode = config.interaction_mode;
106 const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
108 switch ( feedback->event_type )
110 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
111 case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
119 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
122 geometry_msgs::PoseStamped input_pose_stamped;
123 input_pose_stamped.header = feedback->header;
124 input_pose_stamped.pose = feedback->pose;
127 ROS_ERROR(
"Invalid ObjectId Request Received %s", feedback->marker_name.c_str());
140 tobject->
setRGBA(msg.r, msg.g, msg.b, msg.a);
168 if(tobject->
setX(msg)){
178 if(tobject->
setY(msg)){
188 if(tobject->
setZ(msg)){
206 std_msgs::Header
header = msg_ptr->header;
216 geometry_msgs::PoseStamped transformed_pose_stamped;
217 if(req.target_name.compare(std::string(
"")) == 0){
225 transformed_pose_stamped.header.frame_id = tobject->
frame_id_;
226 transformed_pose_stamped.pose = tobject->
getPose(for_interactive_control);
227 res.pose_stamped = transformed_pose_stamped;
234 geometry_msgs::PoseStamped transformed_pose_stamped;
235 if(req.target_name.compare(std::string(
"")) == 0){
244 std_msgs::Header
header = req.pose_stamped.header;
256 if(req.target_name.compare(std::string(
"")) == 0){
263 tobject->
getRGBA(res.color.r, res.color.g, res.color.b, res.color.a);
270 if(req.target_name.compare(std::string(
"")) == 0){
277 tobject->
setRGBA(req.color.r, req.color.g, req.color.b, req.color.a);
300 if(req.target_name.compare(std::string(
"")) == 0){
303 res.type = tobject->
type_;
307 res.type = tobject->
type_;
315 res.existence =
false;
317 res.existence =
true;
325 if(req.target_name.compare(std::string(
"")) == 0){
333 if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
334 tobject->
setXYZ(req.dimensions.x, req.dimensions.y, req.dimensions.z);
335 }
else if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
336 tobject->
setRZ(req.dimensions.radius, req.dimensions.z);
337 }
else if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
338 tobject->
setRSR(req.dimensions.radius, req.dimensions.small_radius);
339 }
else if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
350 if(req.target_name.compare(std::string(
"")) == 0){
358 if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
359 tobject->
getXYZ(res.dimensions.x, res.dimensions.y, res.dimensions.z);
360 }
else if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
361 tobject->
getRZ(res.dimensions.radius, res.dimensions.z);
362 }
else if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
363 tobject->
getRSR(res.dimensions.radius, res.dimensions.small_radius);
364 }
else if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
367 res.dimensions.type = tobject->
getType();
373 std_srvs::Empty::Response& res)
386 std_srvs::Empty::Response& res)
403 jsk_interactive_marker::MarkerDimensions marker_dimensions;
404 if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
405 tobject->
getXYZ(marker_dimensions.x, marker_dimensions.y, marker_dimensions.z);
406 }
else if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
407 tobject->
getRZ(marker_dimensions.radius, marker_dimensions.z);
408 }
else if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
409 tobject->
getRSR(marker_dimensions.radius, marker_dimensions.small_radius);
410 }
else if (tobject->
getType() == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
412 marker_dimensions.type = tobject->
type_;
419 switch(req.operate.action){
420 case jsk_rviz_plugins::TransformableMarkerOperate::INSERT:
422 if (req.operate.name.empty()) {
423 ROS_ERROR(
"Non empty name is required to insert object.");
427 if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
428 insertNewBox(req.operate.frame_id, req.operate.name, req.operate.description);
429 }
else if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
430 insertNewCylinder(req.operate.frame_id, req.operate.name, req.operate.description);
431 }
else if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
432 insertNewTorus(req.operate.frame_id, req.operate.name, req.operate.description);
433 }
else if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
434 insertNewMesh(req.operate.frame_id, req.operate.name, req.operate.description, req.operate.mesh_resource, req.operate.mesh_use_embedded_materials);
438 case jsk_rviz_plugins::TransformableMarkerOperate::ERASE:
442 case jsk_rviz_plugins::TransformableMarkerOperate::ERASEALL:
446 case jsk_rviz_plugins::TransformableMarkerOperate::ERASEFOCUS:
450 case jsk_rviz_plugins::TransformableMarkerOperate::COPY:
453 if (tobject->
type_ == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
458 new_tobject->setXYZ(x, y, z);
459 new_tobject->setPose(tobject->
getPose());
460 }
else if (tobject->
type_ == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
462 tobject->
getRZ(r, z);
465 new_tobject->setRZ(r, z);
466 new_tobject->setPose(tobject->
getPose());
467 }
else if (tobject->
type_ == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
472 new_tobject->setRSR(r, sr);
473 new_tobject->setPose(tobject->
getPose());
474 }
else if (tobject->
type_ == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
475 insertNewMesh(tobject->
frame_id_, req.operate.name, req.operate.description, req.operate.mesh_resource, req.operate.mesh_use_embedded_materials);
477 new_tobject->setPose(tobject->
getPose());
481 new_tobject->setRGBA(r, g, b, a);
518 const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,
528 std::string object_name = it->first;
530 if (
config_.display_interactive_manipulator &&
config_.display_interactive_manipulator_only_selected) {
534 if (
config_.display_description_only_selected) {
543 jsk_rviz_plugins::OverlayText focus_text;
547 focus_text.width = 300;
548 focus_text.height = 50;
549 focus_text.bg_color.r = 0.9;
550 focus_text.bg_color.b = 0.9;
551 focus_text.bg_color.g = 0.9;
552 focus_text.bg_color.a = 0.1;
553 focus_text.fg_color.r = 0.3;
554 focus_text.fg_color.g = 0.3;
555 focus_text.fg_color.b = 0.8;
556 focus_text.fg_color.a = 1;
557 focus_text.line_width = 1;
558 focus_text.text_size = 30;
563 geometry_msgs::Pose target_pose;
564 std::stringstream ss;
567 ss <<
"Pos x: " << target_pose.position.x <<
" y: " << target_pose.position.y <<
" z: " << target_pose.position.z
569 <<
"Ori x: " << target_pose.orientation.x <<
" y: " << target_pose.orientation.y <<
" z: " << target_pose.orientation.z <<
" w: " << target_pose.orientation.w;
572 jsk_rviz_plugins::OverlayText focus_pose;
573 focus_pose.text = ss.str();
576 focus_pose.width = 500;
577 focus_pose.height = 50;
578 focus_pose.bg_color.r = 0.9;
579 focus_pose.bg_color.b = 0.9;
580 focus_pose.bg_color.g = 0.9;
581 focus_pose.bg_color.a = 0.1;
582 focus_pose.fg_color.r = 0.8;
583 focus_pose.fg_color.g = 0.3;
584 focus_pose.fg_color.b = 0.3;
585 focus_pose.fg_color.a = 1;
586 focus_pose.line_width = 1;
587 focus_pose.text_size = 15;
592 std_msgs::String
msg;
611 TransformableTorus* transformable_torus =
new TransformableTorus(0.45, 0.2,
torus_udiv_,
torus_vdiv_, 0.5, 0.5, 0.5, 1.0, frame_id, name, description);
639 if (config.display_interactive_manipulator && !config.display_interactive_manipulator_only_selected) {
640 config.display_interactive_manipulator =
true;
642 config.display_interactive_manipulator =
false;
683 geometry_msgs::PoseStamped transformed_pose_stamped;
684 jsk_interactive_marker::PoseStampedWithName transformed_pose_stamped_with_name;
687 stamp = pose_stamped.header.stamp;
691 pose_stamped.header.stamp =
stamp;
696 tobject->
setPose(transformed_pose_stamped.pose, for_interactive_control);
697 transformed_pose_stamped.pose=tobject->
getPose(
true);
699 transformed_pose_stamped_with_name.pose = transformed_pose_stamped;
700 transformed_pose_stamped_with_name.name = tobject->
name_;
706 pose_stamped.header.frame_id.c_str());
712 ROS_ERROR(
"Transform error: %s", e.what());
717 ROS_ERROR(
"Transform error: %s", e.what());
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ROSCPP_DECL void spin(Spinner &spinner)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
INTERACTIVE_MARKERS_PUBLIC void applyChanges()
INTERACTIVE_MARKERS_PUBLIC bool setPose(const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header=std_msgs::Header())
INTERACTIVE_MARKERS_PUBLIC bool erase(const std::string &name)