00001 #include <jsk_interactive_marker/transformable_interactive_server.h>
00002 #include <jsk_topic_tools/log_utils.h>
00003
00004 using namespace jsk_interactive_marker;
00005
00006 TransformableInteractiveServer::TransformableInteractiveServer():n_(new ros::NodeHandle("~")){
00007 n_->param("interactive_manipulator_orientation", interactive_manipulator_orientation_ , 0);
00008 n_->param("torus_udiv", torus_udiv_, 20);
00009 n_->param("torus_vdiv", torus_vdiv_, 20);
00010 n_->param("strict_tf", strict_tf_, false);
00011 tf_listener_.reset(new tf::TransformListener);
00012 setpose_sub_ = n_->subscribe<geometry_msgs::PoseStamped>("set_pose", 1, boost::bind(&TransformableInteractiveServer::setPose, this, _1, false));
00013 setcontrolpose_sub_ = n_->subscribe<geometry_msgs::PoseStamped>("set_control_pose", 1, boost::bind(&TransformableInteractiveServer::setPose, this, _1, true));
00014 setcolor_sub_ = n_->subscribe("set_color", 1, &TransformableInteractiveServer::setColor, this);
00015
00016 set_r_sub_ = n_->subscribe("set_radius", 1, &TransformableInteractiveServer::setRadius, this);
00017 set_sm_r_sub_ = n_->subscribe("set_small_radius", 1, &TransformableInteractiveServer::setSmallRadius, this);
00018 set_x_sub_ = n_->subscribe("set_x", 1, &TransformableInteractiveServer::setX, this);
00019 set_y_sub_ = n_->subscribe("set_y", 1, &TransformableInteractiveServer::setY, this);
00020 set_z_sub_ = n_->subscribe("set_z", 1, &TransformableInteractiveServer::setZ, this);
00021
00022 addpose_sub_ = n_->subscribe("add_pose", 1, &TransformableInteractiveServer::addPose, this);
00023 addpose_relative_sub_ = n_->subscribe("add_pose_relative", 1, &TransformableInteractiveServer::addPoseRelative, this);
00024
00025 setcontrol_relative_sub_ = n_->subscribe("set_control_relative_pose", 1, &TransformableInteractiveServer::setControlRelativePose, this);
00026
00027 setrad_sub_ = n_->subscribe("set_radius", 1, &TransformableInteractiveServer::setRadius, this);
00028
00029 focus_name_text_pub_ = n_->advertise<jsk_rviz_plugins::OverlayText>("focus_marker_name_text", 1);
00030 focus_pose_text_pub_ = n_->advertise<jsk_rviz_plugins::OverlayText>("focus_marker_pose_text", 1);
00031 focus_object_marker_name_pub_ = n_->advertise<std_msgs::String>("focus_object_marker_name", 1);
00032 pose_pub_ = n_->advertise<geometry_msgs::PoseStamped>("pose", 1);
00033 pose_with_name_pub_ = n_->advertise<jsk_interactive_marker::PoseStampedWithName>("pose_with_name", 1);
00034
00035 get_pose_srv_ = n_->advertiseService<jsk_interactive_marker::GetTransformableMarkerPose::Request, jsk_interactive_marker::GetTransformableMarkerPose::Response>("get_pose", boost::bind(&TransformableInteractiveServer::getPoseService, this, _1, _2, false));
00036 get_control_pose_srv_ = n_->advertiseService<jsk_interactive_marker::GetTransformableMarkerPose::Request, jsk_interactive_marker::GetTransformableMarkerPose::Response>("get_control_pose", boost::bind(&TransformableInteractiveServer::getPoseService, this, _1, _2, true));
00037 set_pose_srv_ = n_->advertiseService<jsk_interactive_marker::SetTransformableMarkerPose::Request ,jsk_interactive_marker::SetTransformableMarkerPose::Response>("set_pose", boost::bind(&TransformableInteractiveServer::setPoseService, this, _1, _2, false));
00038 set_control_pose_srv_ = n_->advertiseService<jsk_interactive_marker::SetTransformableMarkerPose::Request ,jsk_interactive_marker::SetTransformableMarkerPose::Response>("set_control_pose", boost::bind(&TransformableInteractiveServer::setPoseService, this, _1, _2, true));
00039 get_color_srv_ = n_->advertiseService("get_color", &TransformableInteractiveServer::getColorService, this);
00040 set_color_srv_ = n_->advertiseService("set_color", &TransformableInteractiveServer::setColorService, this);
00041 get_focus_srv_ = n_->advertiseService("get_focus", &TransformableInteractiveServer::getFocusService, this);
00042 set_focus_srv_ = n_->advertiseService("set_focus", &TransformableInteractiveServer::setFocusService, this);
00043 get_type_srv_ = n_->advertiseService("get_type", &TransformableInteractiveServer::getTypeService, this);
00044 get_exist_srv_ = n_->advertiseService("get_existence", &TransformableInteractiveServer::getExistenceService, this);
00045 set_dimensions_srv = n_->advertiseService("set_dimensions", &TransformableInteractiveServer::setDimensionsService, this);
00046 get_dimensions_srv = n_->advertiseService("get_dimensions", &TransformableInteractiveServer::getDimensionsService, this);
00047 hide_srv_ = n_->advertiseService("hide", &TransformableInteractiveServer::hideService, this);
00048 show_srv_ = n_->advertiseService("show", &TransformableInteractiveServer::showService, this);
00049 marker_dimensions_pub_ = n_->advertise<jsk_interactive_marker::MarkerDimensions>("marker_dimensions", 1);
00050 request_marker_operate_srv_ = n_->advertiseService("request_marker_operate", &TransformableInteractiveServer::requestMarkerOperateService, this);
00051
00052 config_srv_ = std::make_shared <dynamic_reconfigure::Server<InteractiveSettingConfig> > (*n_);
00053 dynamic_reconfigure::Server<InteractiveSettingConfig>::CallbackType f =
00054 boost::bind (&TransformableInteractiveServer::configCallback, this, _1, _2);
00055 config_srv_->setCallback (f);
00056
00057 tf_timer = n_->createTimer(ros::Duration(0.05), &TransformableInteractiveServer::tfTimerCallback, this);
00058
00059
00060 std::string yaml_filename;
00061 n_->param("yaml_filename", yaml_filename, std::string(""));
00062 yaml_menu_handler_ptr_ = std::make_shared <YamlMenuHandler> (n_, yaml_filename);
00063 yaml_menu_handler_ptr_->_menu_handler.insert(
00064 "enable manipulator",
00065 boost::bind(&TransformableInteractiveServer::enableInteractiveManipulatorDisplay, this, _1, true));
00066 yaml_menu_handler_ptr_->_menu_handler.insert(
00067 "disable manipulator",
00068 boost::bind(&TransformableInteractiveServer::enableInteractiveManipulatorDisplay, this, _1, false));
00069
00070 bool use_parent_and_child;
00071 n_->param("use_parent_and_child", use_parent_and_child, false);
00072 if (use_parent_and_child)
00073 {
00074 ROS_INFO("initialize parent and child marker");
00075 server_ = new jsk_interactive_marker::ParentAndChildInteractiveMarkerServer("simple_marker");
00076 }
00077 else
00078 {
00079 ROS_INFO("initialize simple marker");
00080 server_ = new interactive_markers::InteractiveMarkerServer("simple_marker");
00081 }
00082 }
00083
00084 TransformableInteractiveServer::~TransformableInteractiveServer()
00085 {
00086 for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin(); itpairstri != transformable_objects_map_.end(); itpairstri++) {
00087 delete itpairstri->second;
00088 }
00089 transformable_objects_map_.clear();
00090 }
00091
00092 void TransformableInteractiveServer::configCallback(InteractiveSettingConfig &config, uint32_t level)
00093 {
00094 boost::mutex::scoped_lock lock(mutex_);
00095 config_ = config;
00096 int interaction_mode = config.interaction_mode;
00097 for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin(); itpairstri != transformable_objects_map_.end(); itpairstri++) {
00098 TransformableObject* tobject = itpairstri->second;
00099 tobject->setInteractiveMarkerSetting(config_);
00100 updateTransformableObject(tobject);
00101 }
00102 }
00103
00104
00105 void TransformableInteractiveServer::processFeedback(
00106 const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00107 {
00108 switch ( feedback->event_type )
00109 {
00110 case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
00111 case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
00112 focus_object_marker_name_ = feedback->marker_name;
00113 focusTextPublish();
00114 focusPosePublish();
00115 focusObjectMarkerNamePublish();
00116 focusInteractiveManipulatorDisplay();
00117 break;
00118
00119 case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
00120 TransformableObject* tobject = transformable_objects_map_[feedback->marker_name.c_str()];
00121 if(tobject){
00122 geometry_msgs::PoseStamped input_pose_stamped;
00123 input_pose_stamped.header = feedback->header;
00124 input_pose_stamped.pose = feedback->pose;
00125 setPoseWithTfTransformation(tobject, input_pose_stamped, true);
00126 }else{
00127 ROS_ERROR("Invalid ObjectId Request Received %s", feedback->marker_name.c_str());
00128 }
00129 focusTextPublish();
00130 focusPosePublish();
00131 focusObjectMarkerNamePublish();
00132 break;
00133 }
00134 }
00135
00136 void TransformableInteractiveServer::setColor(std_msgs::ColorRGBA msg)
00137 {
00138 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00139 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00140 tobject->setRGBA(msg.r, msg.g, msg.b, msg.a);
00141 updateTransformableObject(tobject);
00142 }
00143
00144 void TransformableInteractiveServer::setRadius(std_msgs::Float32 msg)
00145 {
00146 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00147 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00148 if(tobject->setRadius(msg)){
00149 updateTransformableObject(tobject);
00150 publishMarkerDimensions();
00151 }
00152 }
00153
00154 void TransformableInteractiveServer::setSmallRadius(std_msgs::Float32 msg)
00155 {
00156 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00157 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00158 if(tobject->setSmallRadius(msg)){
00159 updateTransformableObject(tobject);
00160 publishMarkerDimensions();
00161 }
00162 }
00163
00164 void TransformableInteractiveServer::setX(std_msgs::Float32 msg)
00165 {
00166 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00167 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00168 if(tobject->setX(msg)){
00169 updateTransformableObject(tobject);
00170 publishMarkerDimensions();
00171 }
00172 }
00173
00174 void TransformableInteractiveServer::setY(std_msgs::Float32 msg)
00175 {
00176 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00177 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00178 if(tobject->setY(msg)){
00179 updateTransformableObject(tobject);
00180 publishMarkerDimensions();
00181 }
00182 }
00183
00184 void TransformableInteractiveServer::setZ(std_msgs::Float32 msg)
00185 {
00186 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00187 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00188 if(tobject->setZ(msg)){
00189 updateTransformableObject(tobject);
00190 publishMarkerDimensions();
00191 }
00192 }
00193
00194 void TransformableInteractiveServer::updateTransformableObject(TransformableObject* tobject)
00195 {
00196 visualization_msgs::InteractiveMarker int_marker = tobject->getInteractiveMarker();
00197 server_->insert(int_marker, boost::bind( &TransformableInteractiveServer::processFeedback,this, _1));
00198 yaml_menu_handler_ptr_->applyMenu(server_, focus_object_marker_name_);
00199 server_->applyChanges();
00200 }
00201
00202 void TransformableInteractiveServer::setPose(const geometry_msgs::PoseStampedConstPtr &msg_ptr, bool for_interactive_control){
00203 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00204 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00205 setPoseWithTfTransformation(tobject, *msg_ptr, for_interactive_control);
00206 std_msgs::Header header = msg_ptr->header;
00207 header.frame_id = tobject->getFrameId();
00208 server_->setPose(focus_object_marker_name_, tobject->pose_, header);
00209 yaml_menu_handler_ptr_->applyMenu(server_, focus_object_marker_name_);
00210 server_->applyChanges();
00211 }
00212
00213 bool TransformableInteractiveServer::getPoseService(jsk_interactive_marker::GetTransformableMarkerPose::Request &req,jsk_interactive_marker::GetTransformableMarkerPose::Response &res, bool for_interactive_control)
00214 {
00215 TransformableObject* tobject;
00216 geometry_msgs::PoseStamped transformed_pose_stamped;
00217 if(req.target_name.compare(std::string("")) == 0){
00218 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return true; }
00219 tobject = transformable_objects_map_[focus_object_marker_name_];
00220 }else{
00221 if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
00222 tobject = transformable_objects_map_[req.target_name];
00223 }
00224 transformed_pose_stamped.header.stamp = ros::Time::now();
00225 transformed_pose_stamped.header.frame_id = tobject->frame_id_;
00226 transformed_pose_stamped.pose = tobject->getPose(for_interactive_control);
00227 res.pose_stamped = transformed_pose_stamped;
00228 return true;
00229 }
00230
00231 bool TransformableInteractiveServer::setPoseService(jsk_interactive_marker::SetTransformableMarkerPose::Request &req,jsk_interactive_marker::SetTransformableMarkerPose::Response &res, bool for_interactive_control)
00232 {
00233 TransformableObject* tobject;
00234 geometry_msgs::PoseStamped transformed_pose_stamped;
00235 if(req.target_name.compare(std::string("")) == 0){
00236 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return true; }
00237 tobject = transformable_objects_map_[focus_object_marker_name_];
00238 }else{
00239 if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
00240 focus_object_marker_name_ = req.target_name;
00241 tobject = transformable_objects_map_[req.target_name];
00242 }
00243 if(setPoseWithTfTransformation(tobject, req.pose_stamped, for_interactive_control)){
00244 std_msgs::Header header = req.pose_stamped.header;
00245 header.frame_id = tobject->getFrameId();
00246 server_->setPose(focus_object_marker_name_, tobject->pose_, header);
00247 yaml_menu_handler_ptr_->applyMenu(server_, focus_object_marker_name_);
00248 server_->applyChanges();
00249 }
00250 return true;
00251 }
00252
00253 bool TransformableInteractiveServer::getColorService(jsk_interactive_marker::GetTransformableMarkerColor::Request &req,jsk_interactive_marker::GetTransformableMarkerColor::Response &res)
00254 {
00255 TransformableObject* tobject;
00256 if(req.target_name.compare(std::string("")) == 0){
00257 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return true; }
00258 tobject = transformable_objects_map_[focus_object_marker_name_];
00259 }else{
00260 if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
00261 tobject = transformable_objects_map_[req.target_name];
00262 }
00263 tobject->getRGBA(res.color.r, res.color.g, res.color.b, res.color.a);
00264 return true;
00265 }
00266
00267 bool TransformableInteractiveServer::setColorService(jsk_interactive_marker::SetTransformableMarkerColor::Request &req,jsk_interactive_marker::SetTransformableMarkerColor::Response &res)
00268 {
00269 TransformableObject* tobject;
00270 if(req.target_name.compare(std::string("")) == 0){
00271 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return true; }
00272 tobject = transformable_objects_map_[focus_object_marker_name_];
00273 }else{
00274 if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
00275 tobject = transformable_objects_map_[req.target_name];
00276 }
00277 tobject->setRGBA(req.color.r, req.color.g, req.color.b, req.color.a);
00278 updateTransformableObject(tobject);
00279 return true;
00280 }
00281
00282 bool TransformableInteractiveServer::getFocusService(jsk_interactive_marker::GetTransformableMarkerFocus::Request &req,jsk_interactive_marker::GetTransformableMarkerFocus::Response &res)
00283 {
00284 res.target_name = focus_object_marker_name_;
00285 return true;
00286 }
00287
00288 bool TransformableInteractiveServer::setFocusService(jsk_interactive_marker::SetTransformableMarkerFocus::Request &req,jsk_interactive_marker::SetTransformableMarkerFocus::Response &res)
00289 {
00290 focus_object_marker_name_ = req.target_name;
00291 focusTextPublish();
00292 focusPosePublish();
00293 focusObjectMarkerNamePublish();
00294 return true;
00295 }
00296
00297 bool TransformableInteractiveServer::getTypeService(jsk_interactive_marker::GetType::Request &req,jsk_interactive_marker::GetType::Response &res)
00298 {
00299 TransformableObject* tobject;
00300 if(req.target_name.compare(std::string("")) == 0){
00301 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return true; }
00302 tobject = transformable_objects_map_[focus_object_marker_name_];
00303 res.type = tobject->type_;
00304 }else{
00305 if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
00306 tobject = transformable_objects_map_[req.target_name];
00307 res.type = tobject->type_;
00308 }
00309 return true;
00310 }
00311
00312 bool TransformableInteractiveServer::getExistenceService(jsk_interactive_marker::GetTransformableMarkerExistence::Request &req,jsk_interactive_marker::GetTransformableMarkerExistence::Response &res)
00313 {
00314 if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) {
00315 res.existence = false;
00316 } else {
00317 res.existence = true;
00318 }
00319 return true;
00320 }
00321
00322 bool TransformableInteractiveServer::setDimensionsService(jsk_interactive_marker::SetMarkerDimensions::Request &req,jsk_interactive_marker::SetMarkerDimensions::Response &res)
00323 {
00324 TransformableObject* tobject;
00325 if(req.target_name.compare(std::string("")) == 0){
00326 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return true; }
00327 tobject = transformable_objects_map_[focus_object_marker_name_];
00328 }else{
00329 if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
00330 tobject = transformable_objects_map_[req.target_name];
00331 }
00332 if (tobject) {
00333 if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
00334 tobject->setXYZ(req.dimensions.x, req.dimensions.y, req.dimensions.z);
00335 } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
00336 tobject->setRZ(req.dimensions.radius, req.dimensions.z);
00337 } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
00338 tobject->setRSR(req.dimensions.radius, req.dimensions.small_radius);
00339 } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
00340 }
00341 publishMarkerDimensions();
00342 updateTransformableObject(tobject);
00343 }
00344 return true;
00345 }
00346
00347 bool TransformableInteractiveServer::getDimensionsService(jsk_interactive_marker::GetMarkerDimensions::Request &req,jsk_interactive_marker::GetMarkerDimensions::Response &res)
00348 {
00349 TransformableObject* tobject;
00350 if(req.target_name.compare(std::string("")) == 0){
00351 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return true; }
00352 tobject = transformable_objects_map_[focus_object_marker_name_];
00353 }else{
00354 if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
00355 tobject = transformable_objects_map_[req.target_name];
00356 }
00357 if (tobject) {
00358 if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
00359 tobject->getXYZ(res.dimensions.x, res.dimensions.y, res.dimensions.z);
00360 } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
00361 tobject->getRZ(res.dimensions.radius, res.dimensions.z);
00362 } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
00363 tobject->getRSR(res.dimensions.radius, res.dimensions.small_radius);
00364 } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
00365 }
00366
00367 res.dimensions.type = tobject->getType();
00368 }
00369 return true;
00370 }
00371
00372 bool TransformableInteractiveServer::hideService(std_srvs::Empty::Request& req,
00373 std_srvs::Empty::Response& res)
00374 {
00375 for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin();
00376 itpairstri != transformable_objects_map_.end();
00377 ++itpairstri) {
00378 TransformableObject* tobject = itpairstri->second;
00379 tobject->setDisplayInteractiveManipulator(false);
00380 updateTransformableObject(tobject);
00381 }
00382 return true;
00383 }
00384
00385 bool TransformableInteractiveServer::showService(std_srvs::Empty::Request& req,
00386 std_srvs::Empty::Response& res)
00387 {
00388 for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin();
00389 itpairstri != transformable_objects_map_.end();
00390 ++itpairstri) {
00391 TransformableObject* tobject = itpairstri->second;
00392 tobject->setDisplayInteractiveManipulator(true);
00393 updateTransformableObject(tobject);
00394 }
00395 return true;
00396 }
00397 void TransformableInteractiveServer::publishMarkerDimensions()
00398 {
00399 TransformableObject* tobject;
00400 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00401 tobject = transformable_objects_map_[focus_object_marker_name_];
00402 if (tobject) {
00403 jsk_interactive_marker::MarkerDimensions marker_dimensions;
00404 if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
00405 tobject->getXYZ(marker_dimensions.x, marker_dimensions.y, marker_dimensions.z);
00406 } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
00407 tobject->getRZ(marker_dimensions.radius, marker_dimensions.z);
00408 } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
00409 tobject->getRSR(marker_dimensions.radius, marker_dimensions.small_radius);
00410 } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
00411 }
00412 marker_dimensions.type = tobject->type_;
00413 marker_dimensions_pub_.publish(marker_dimensions);
00414 }
00415 }
00416
00417 bool TransformableInteractiveServer::requestMarkerOperateService(jsk_rviz_plugins::RequestMarkerOperate::Request &req,jsk_rviz_plugins::RequestMarkerOperate::Response &res)
00418 {
00419 switch(req.operate.action){
00420 case jsk_rviz_plugins::TransformableMarkerOperate::INSERT:
00421
00422 if (req.operate.name.empty()) {
00423 ROS_ERROR("Non empty name is required to insert object.");
00424 return false;
00425 }
00426
00427 if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
00428 insertNewBox(req.operate.frame_id, req.operate.name, req.operate.description);
00429 } else if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
00430 insertNewCylinder(req.operate.frame_id, req.operate.name, req.operate.description);
00431 } else if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
00432 insertNewTorus(req.operate.frame_id, req.operate.name, req.operate.description);
00433 } else if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
00434 insertNewMesh(req.operate.frame_id, req.operate.name, req.operate.description, req.operate.mesh_resource, req.operate.mesh_use_embedded_materials);
00435 }
00436 return true;
00437 break;
00438 case jsk_rviz_plugins::TransformableMarkerOperate::ERASE:
00439 eraseObject(req.operate.name);
00440 return true;
00441 break;
00442 case jsk_rviz_plugins::TransformableMarkerOperate::ERASEALL:
00443 eraseAllObject();
00444 return true;
00445 break;
00446 case jsk_rviz_plugins::TransformableMarkerOperate::ERASEFOCUS:
00447 eraseFocusObject();
00448 return true;
00449 break;
00450 case jsk_rviz_plugins::TransformableMarkerOperate::COPY:
00451 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return true; }
00452 TransformableObject *tobject = transformable_objects_map_[focus_object_marker_name_], *new_tobject;
00453 if (tobject->type_ == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
00454 float x, y, z;
00455 tobject->getXYZ(x, y, z);
00456 insertNewBox(tobject->frame_id_, req.operate.name, req.operate.description);
00457 new_tobject = transformable_objects_map_[req.operate.name];
00458 new_tobject->setXYZ(x, y, z);
00459 new_tobject->setPose(tobject->getPose());
00460 } else if (tobject->type_ == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
00461 float r, z;
00462 tobject->getRZ(r, z);
00463 insertNewCylinder(tobject->frame_id_, req.operate.name, req.operate.description);
00464 new_tobject = transformable_objects_map_[req.operate.name];
00465 new_tobject->setRZ(r, z);
00466 new_tobject->setPose(tobject->getPose());
00467 } else if (tobject->type_ == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
00468 float r, sr;
00469 tobject->getRSR(r, sr);
00470 insertNewTorus(tobject->frame_id_, req.operate.name, req.operate.description);
00471 new_tobject = transformable_objects_map_[req.operate.name];
00472 new_tobject->setRSR(r, sr);
00473 new_tobject->setPose(tobject->getPose());
00474 } else if (tobject->type_ == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
00475 insertNewMesh(tobject->frame_id_, req.operate.name, req.operate.description, req.operate.mesh_resource, req.operate.mesh_use_embedded_materials);
00476 new_tobject = transformable_objects_map_[req.operate.name];
00477 new_tobject->setPose(tobject->getPose());
00478 }
00479 float r, g, b, a;
00480 tobject->getRGBA(r, g, b, a);
00481 new_tobject->setRGBA(r, g, b, a);
00482 return true;
00483 break;
00484 };
00485 return false;
00486 }
00487
00488 void TransformableInteractiveServer::addPose(geometry_msgs::Pose msg){
00489 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00490 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00491 tobject->addPose(msg,false);
00492 updateTransformableObject(tobject);
00493 }
00494
00495 void TransformableInteractiveServer::addPoseRelative(geometry_msgs::Pose msg){
00496 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00497 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00498 tobject->addPose(msg,true);
00499 updateTransformableObject(tobject);
00500 }
00501
00502 void TransformableInteractiveServer::setControlRelativePose(geometry_msgs::Pose msg){
00503 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00504 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00505 geometry_msgs::Pose pose = tobject->getPose();
00506 tobject->control_offset_pose_ = msg;
00507 updateTransformableObject(tobject);
00508 tobject->setPose(pose);
00509 std_msgs::Header header;
00510 header.frame_id = tobject->getFrameId();
00511 header.stamp = ros::Time::now();
00512 server_->setPose(focus_object_marker_name_, tobject->pose_, header);
00513 yaml_menu_handler_ptr_->applyMenu(server_, focus_object_marker_name_);
00514 server_->applyChanges();
00515 }
00516
00517 void TransformableInteractiveServer::enableInteractiveManipulatorDisplay(
00518 const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,
00519 const bool enable) {
00520 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00521 tobject->setDisplayInteractiveManipulator(enable);
00522 updateTransformableObject(tobject);
00523 }
00524
00525 void TransformableInteractiveServer::focusInteractiveManipulatorDisplay() {
00526 for (std::map<string, TransformableObject* >::iterator it = transformable_objects_map_.begin();
00527 it != transformable_objects_map_.end(); it++) {
00528 std::string object_name = it->first;
00529 TransformableObject* tobject = it->second;
00530 if (config_.display_interactive_manipulator && config_.display_interactive_manipulator_only_selected) {
00531
00532 tobject->setDisplayInteractiveManipulator(object_name == focus_object_marker_name_);
00533 }
00534 if (config_.display_description_only_selected) {
00535
00536 tobject->setDisplayDescription(object_name == focus_object_marker_name_);
00537 }
00538 updateTransformableObject(tobject);
00539 }
00540 }
00541
00542 void TransformableInteractiveServer::focusTextPublish(){
00543 jsk_rviz_plugins::OverlayText focus_text;
00544 focus_text.text = focus_object_marker_name_;
00545 focus_text.top = 0;
00546 focus_text.left = 0;
00547 focus_text.width = 300;
00548 focus_text.height = 50;
00549 focus_text.bg_color.r = 0.9;
00550 focus_text.bg_color.b = 0.9;
00551 focus_text.bg_color.g = 0.9;
00552 focus_text.bg_color.a = 0.1;
00553 focus_text.fg_color.r = 0.3;
00554 focus_text.fg_color.g = 0.3;
00555 focus_text.fg_color.b = 0.8;
00556 focus_text.fg_color.a = 1;
00557 focus_text.line_width = 1;
00558 focus_text.text_size = 30;
00559 focus_name_text_pub_.publish(focus_text);
00560 }
00561
00562 void TransformableInteractiveServer::focusPosePublish(){
00563 geometry_msgs::Pose target_pose;
00564 std::stringstream ss;
00565 if (transformable_objects_map_.find(focus_object_marker_name_) != transformable_objects_map_.end()) {
00566 target_pose = transformable_objects_map_[focus_object_marker_name_]->getPose();
00567 ss << "Pos x: " << target_pose.position.x << " y: " << target_pose.position.y << " z: " << target_pose.position.z
00568 << std::endl
00569 << "Ori x: " << target_pose.orientation.x << " y: " << target_pose.orientation.y << " z: " << target_pose.orientation.z << " w: " << target_pose.orientation.w;
00570 }
00571
00572 jsk_rviz_plugins::OverlayText focus_pose;
00573 focus_pose.text = ss.str();
00574 focus_pose.top = 50;
00575 focus_pose.left = 0;
00576 focus_pose.width = 500;
00577 focus_pose.height = 50;
00578 focus_pose.bg_color.r = 0.9;
00579 focus_pose.bg_color.b = 0.9;
00580 focus_pose.bg_color.g = 0.9;
00581 focus_pose.bg_color.a = 0.1;
00582 focus_pose.fg_color.r = 0.8;
00583 focus_pose.fg_color.g = 0.3;
00584 focus_pose.fg_color.b = 0.3;
00585 focus_pose.fg_color.a = 1;
00586 focus_pose.line_width = 1;
00587 focus_pose.text_size = 15;
00588 focus_pose_text_pub_.publish(focus_pose);
00589 }
00590
00591 void TransformableInteractiveServer::focusObjectMarkerNamePublish(){
00592 std_msgs::String msg;
00593 msg.data = focus_object_marker_name_;
00594 focus_object_marker_name_pub_.publish(msg);
00595 }
00596
00597 void TransformableInteractiveServer::insertNewBox(std::string frame_id, std::string name, std::string description)
00598 {
00599 TransformableBox* transformable_box = new TransformableBox(0.45, 0.45, 0.45, 0.5, 0.5, 0.5, 1.0, frame_id, name, description);
00600 insertNewObject(transformable_box, name);
00601 }
00602
00603 void TransformableInteractiveServer::insertNewCylinder( std::string frame_id, std::string name, std::string description)
00604 {
00605 TransformableCylinder* transformable_cylinder = new TransformableCylinder(0.45, 0.45, 0.5, 0.5, 0.5, 1.0, frame_id, name, description);
00606 insertNewObject(transformable_cylinder, name);
00607 }
00608
00609 void TransformableInteractiveServer::insertNewTorus( std::string frame_id, std::string name, std::string description)
00610 {
00611 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);
00612 insertNewObject(transformable_torus, name);
00613 }
00614
00615 void TransformableInteractiveServer::insertNewMesh( std::string frame_id, std::string name, std::string description, std::string mesh_resource, bool mesh_use_embedded_materials)
00616 {
00617 TransformableMesh* transformable_mesh = new TransformableMesh(frame_id, name, description, mesh_resource, mesh_use_embedded_materials);
00618 insertNewObject(transformable_mesh, name);
00619 }
00620
00621 void TransformableInteractiveServer::insertNewObject( TransformableObject* tobject , std::string name )
00622 {
00623 SetInitialInteractiveMarkerConfig(tobject);
00624 visualization_msgs::InteractiveMarker int_marker = tobject->getInteractiveMarker();
00625 transformable_objects_map_[name] = tobject;
00626 server_->insert(int_marker, boost::bind( &TransformableInteractiveServer::processFeedback,this, _1));
00627 yaml_menu_handler_ptr_->applyMenu(server_, name);
00628 server_->applyChanges();
00629
00630 focus_object_marker_name_ = name;
00631 focusTextPublish();
00632 focusPosePublish();
00633 focusObjectMarkerNamePublish();
00634 }
00635
00636 void TransformableInteractiveServer::SetInitialInteractiveMarkerConfig( TransformableObject* tobject )
00637 {
00638 InteractiveSettingConfig config(config_);
00639 if (config.display_interactive_manipulator && !config.display_interactive_manipulator_only_selected) {
00640 config.display_interactive_manipulator = true;
00641 } else {
00642 config.display_interactive_manipulator = false;
00643 }
00644 tobject->setInteractiveMarkerSetting(config);
00645 }
00646
00647 void TransformableInteractiveServer::eraseObject( std::string name )
00648 {
00649 server_->erase(name);
00650 server_->applyChanges();
00651 if (focus_object_marker_name_.compare(name) == 0) {
00652 focus_object_marker_name_ = "";
00653 focusTextPublish();
00654 focusPosePublish();
00655 focusObjectMarkerNamePublish();
00656 }
00657 delete transformable_objects_map_[name];
00658 transformable_objects_map_.erase(name);
00659 }
00660
00661 void TransformableInteractiveServer::eraseAllObject()
00662 {
00663 for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin(); itpairstri != transformable_objects_map_.end(); itpairstri++) {
00664 eraseObject(itpairstri->first);
00665 }
00666 }
00667
00668 void TransformableInteractiveServer::eraseFocusObject()
00669 {
00670 eraseObject(focus_object_marker_name_);
00671 }
00672
00673 void TransformableInteractiveServer::tfTimerCallback(const ros::TimerEvent&)
00674 {
00675 if (transformable_objects_map_.find(focus_object_marker_name_) == transformable_objects_map_.end()) { return; }
00676 TransformableObject* tobject = transformable_objects_map_[focus_object_marker_name_];
00677 tobject->publishTF();
00678 }
00679
00680 bool TransformableInteractiveServer::setPoseWithTfTransformation(TransformableObject* tobject, geometry_msgs::PoseStamped pose_stamped, bool for_interactive_control)
00681 {
00682 try {
00683 geometry_msgs::PoseStamped transformed_pose_stamped;
00684 jsk_interactive_marker::PoseStampedWithName transformed_pose_stamped_with_name;
00685 ros::Time stamp;
00686 if (strict_tf_) {
00687 stamp = pose_stamped.header.stamp;
00688 }
00689 else {
00690 stamp = ros::Time(0.0);
00691 pose_stamped.header.stamp = stamp;
00692 }
00693 if (!strict_tf_ || tf_listener_->waitForTransform(tobject->getFrameId(),
00694 pose_stamped.header.frame_id, stamp, ros::Duration(1.0))) {
00695 tf_listener_->transformPose(tobject->getFrameId(), pose_stamped, transformed_pose_stamped);
00696 tobject->setPose(transformed_pose_stamped.pose, for_interactive_control);
00697 transformed_pose_stamped.pose=tobject->getPose(true);
00698 pose_pub_.publish(transformed_pose_stamped);
00699 transformed_pose_stamped_with_name.pose = transformed_pose_stamped;
00700 transformed_pose_stamped_with_name.name = tobject->name_;
00701
00702 pose_with_name_pub_.publish(transformed_pose_stamped_with_name);
00703 }
00704 else {
00705 ROS_ERROR("failed to lookup transform %s -> %s", tobject->getFrameId().c_str(),
00706 pose_stamped.header.frame_id.c_str());
00707 return false;
00708 }
00709 }
00710 catch (tf2::ConnectivityException &e)
00711 {
00712 ROS_ERROR("Transform error: %s", e.what());
00713 return false;
00714 }
00715 catch (tf2::InvalidArgumentException &e)
00716 {
00717 ROS_ERROR("Transform error: %s", e.what());
00718 return false;
00719 }
00720 return true;
00721 }
00722
00723 void TransformableInteractiveServer::run(){
00724 ros::spin();
00725 }