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