void goalCB() { // accept the new goal ROS_INFO("New goal"); goal_ = as_.acceptNewGoal(); std::string topic_suffix = BUT_PositionClicked_TOPIC(goal_->topic_suffix); position_clicked_subscriber_ = nh_.subscribe(topic_suffix, 1, &ClickablePositionsAction::positionClickedCallback, this); srs_interaction_primitives::ClickablePositions srv; srv.request.color = goal_->color; srv.request.frame_id = goal_->frame_id; srv.request.positions = goal_->positions; srv.request.radius = goal_->radius; srv.request.topic_suffix = goal_->topic_suffix; ros::service::waitForService(srs_interaction_primitives::ClickablePositions_SRV); if (clickable_positions_client_.call(srv)) { } else { ROS_ERROR("Clickable positions not added"); as_.setAborted(); } }
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; }