bool addMarker(AddMarker::Request &req, AddMarker::Response &res)
{
ROS_INFO("ADDING MARKER");
InteractiveMarker tmp;
if ((imServer->get(req.name, tmp)) || (primitives.count(req.name) != 0))
{
ROS_ERROR("Object with that name already exists! Please remove it first.");
return false;
}
InteractiveMarker object;
Marker marker;
marker.type = req.type;
marker.color = req.color;
marker.scale = req.scale;
object.header.frame_id = req.frame_id;
object.header.stamp = ros::Time::now();
object.name = req.name;
object.description = req.description;
object.pose = req.pose;
InteractiveMarkerControl control;
control.name = object.name + "_control";
control.interaction_mode = InteractiveMarkerControl::NONE;
control.always_visible = true;
control.markers.push_back(marker);
object.controls.push_back(control);
imServer->insert(object);
imServer->applyChanges();
ROS_INFO("..... DONE");
return true;
}
bool clickablePositions(ClickablePositions::Request &req, ClickablePositions::Response &res)
{
ROS_INFO("ADDING CLICK POSITIONS");
std::vector<geometry_msgs::Point> positions;
for (unsigned int i = 0; i < req.positions.size(); i++)
{
positions.push_back(req.positions.at(i));
}
ClickablePositionsMarker *clickPositions = new ClickablePositionsMarker(imServer,req.frame_id, req.topic_suffix, req.radius,
req.color, positions);
imServer->insert(clickPositions->getMarker(),
boost::bind(&ClickablePositionsMarker::markerFeedback, clickPositions, _1));
imServer->applyChanges();
res.topic = BUT_PositionClicked_TOPIC(req.topic_suffix);
ROS_INFO("DONE");
return true;
}
