46 const std::string& parent_frame)
47 : nh_(
"~"), latest_pose_(initial_pose)
50 const std::string imarker_topic = nh_.getNamespace() +
"/" + name;
59 imarker_server_->applyChanges();
62 geometry_msgs::Pose& IMarkerSimple::getPose()
67 void IMarkerSimple::setPose(
const Eigen::Isometry3d& pose)
69 geometry_msgs::Pose pose_msg;
74 void IMarkerSimple::setPose(
const geometry_msgs::Pose& pose)
77 sendUpdatedIMarkerPose();
80 void IMarkerSimple::iMarkerCallback(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
83 if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
88 latest_pose_ = feedback->pose;
91 if (imarker_callback_)
92 imarker_callback_(feedback);
95 void IMarkerSimple::sendUpdatedIMarkerPose()
97 imarker_server_->setPose(int_marker_.name, latest_pose_);
98 imarker_server_->applyChanges();
101 void IMarkerSimple::make6DofMarker(
const geometry_msgs::Pose& pose,
double scale,
const std::string& parent_frame)
105 int_marker_.header.frame_id = parent_frame;
106 int_marker_.pose = pose;
107 int_marker_.scale = scale;
109 int_marker_.name = name_;
113 InteractiveMarkerControl control;
114 control.orientation.w = 0.7071;
115 control.orientation.x = 0.7071;
116 control.orientation.y = 0;
117 control.orientation.z = 0;
118 control.name =
"rotate_x";
119 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
120 int_marker_.controls.push_back(control);
121 control.name =
"move_x";
122 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
123 int_marker_.controls.push_back(control);
125 control.orientation.w = 0.7071;
126 control.orientation.x = 0;
127 control.orientation.y = 0.7071;
128 control.orientation.z = 0;
129 control.name =
"rotate_z";
130 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
131 int_marker_.controls.push_back(control);
132 control.name =
"move_z";
133 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
134 int_marker_.controls.push_back(control);
136 control.orientation.w = 0.7071;
137 control.orientation.x = 0;
138 control.orientation.y = 0;
139 control.orientation.z = 0.7071;
140 control.name =
"rotate_y";
141 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
142 int_marker_.controls.push_back(control);
143 control.name =
"move_y";
144 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
145 int_marker_.controls.push_back(control);
147 imarker_server_->insert(int_marker_);
148 imarker_server_->setCallback(int_marker_.name, boost::bind(&IMarkerSimple::iMarkerCallback,
this, _1));