$search
00001 /* 00002 * Copyright (c) 2011, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include <ros/ros.h> 00031 00032 #include <actionlib/client/simple_action_client.h> 00033 #include <simple_arm_server/MoveArmAction.h> 00034 #include <arbotix_msgs/Relax.h> 00035 00036 #include <interactive_markers/interactive_marker_server.h> 00037 #include <interactive_markers/menu_handler.h> 00038 00039 #include <geometry_msgs/Twist.h> 00040 #include <geometry_msgs/Pose.h> 00041 #include <std_msgs/Float64.h> 00042 00043 #include <tf/tf.h> 00044 #include <tf/transform_listener.h> 00045 00046 #include <string.h> 00047 00048 #include <boost/foreach.hpp> 00049 00050 using namespace visualization_msgs; 00051 using namespace interactive_markers; 00052 using namespace std; 00053 00054 class TurtlebotArmMarkerServer 00055 { 00056 private: 00057 ros::NodeHandle nh; 00058 actionlib::SimpleActionClient<simple_arm_server::MoveArmAction> client; 00059 interactive_markers::InteractiveMarkerServer server; 00060 tf::TransformListener tf_listener; 00061 00062 MenuHandler arm_menu_handler; 00063 map<std::string, MenuHandler> joint_menu_handlers; 00064 00065 bool immediate_commands; 00066 bool in_move; 00067 00068 ros::Timer arm_timer; 00069 00070 // Joint and link parameters 00071 vector<string> joints; 00072 vector<string> links; 00073 string tip_link; 00074 string root_link; 00075 00076 // Gripper parameters 00077 double gripper_marker_offset_x; 00078 double gripper_marker_offset_y; 00079 double gripper_marker_offset_z; 00080 00081 double gripper_box_offset_x; 00082 double gripper_box_offset_y; 00083 double gripper_box_offset_z; 00084 00085 // Other parameters 00086 double move_time; 00087 00088 // Joint command and relax publishers 00089 map<std::string, ros::Publisher> joint_command_publishers; 00090 map<std::string, ros::ServiceClient> joint_relax_clients; 00091 00092 public: 00093 TurtlebotArmMarkerServer() 00094 : nh("~"), client("move_arm", true), server("turtlebot_arm_marker_server"), tf_listener(nh), immediate_commands(true), in_move(false) 00095 { 00096 // Get general arm parameters 00097 nh.param<std::string>("root_link", root_link, "/arm_base_link"); 00098 nh.param<std::string>("tip_link", tip_link, "/gripper_link"); 00099 nh.param<double>("move_time", move_time, 2.0); 00100 00101 // Get the joint list 00102 XmlRpc::XmlRpcValue joint_list; 00103 if (nh.getParam("joints", joint_list)) 00104 { 00105 ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 00106 00107 for (int32_t i = 0; i < joint_list.size(); ++i) 00108 { 00109 ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString); 00110 joints.push_back(joint_list[i]); 00111 } 00112 } 00113 else 00114 { 00115 joints.push_back("arm_shoulder_pan_joint"); 00116 joints.push_back("arm_shoulder_lift_joint"); 00117 joints.push_back("arm_elbow_flex_joint"); 00118 joints.push_back("arm_wrist_flex_joint"); 00119 joints.push_back("gripper_joint"); 00120 } 00121 00122 // Get the corresponding link list 00123 XmlRpc::XmlRpcValue link_list; 00124 if (nh.getParam("links", link_list)) 00125 { 00126 ROS_ASSERT(link_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 00127 00128 for (int32_t i = 0; i < joint_list.size(); ++i) 00129 { 00130 ROS_ASSERT(link_list[i].getType() == XmlRpc::XmlRpcValue::TypeString); 00131 links.push_back(link_list[i]); 00132 } 00133 } 00134 else 00135 { 00136 links.push_back("arm_shoulder_pan_servo_link"); 00137 links.push_back("arm_shoulder_lift_servo_link"); 00138 links.push_back("arm_elbow_flex_servo_link"); 00139 links.push_back("arm_wrist_flex_servo_link"); 00140 links.push_back("gripper_servo_link"); 00141 } 00142 00143 // Get gripper offsets 00144 nh.param<double>("gripper/marker_offset/x", gripper_marker_offset_x, 0.02); 00145 nh.param<double>("gripper/marker_offset/y", gripper_marker_offset_y, -0.025); 00146 nh.param<double>("gripper/marker_offset/z", gripper_marker_offset_z, 0.0); 00147 00148 nh.param<double>("gripper/box_offset/x", gripper_box_offset_x, -0.017); 00149 nh.param<double>("gripper/box_offset/y", gripper_box_offset_y, 0.008); 00150 nh.param<double>("gripper/box_offset/z", gripper_box_offset_z, 0.0); 00151 00152 createArmMarker(); 00153 createGripperMarker(); 00154 createArmMenu(); 00155 00156 createJointPublishers(); 00157 00158 resetMarker(); 00159 00160 ROS_INFO("[turtlebot arm marker server] Initialized."); 00161 } 00162 00163 void createJointPublishers() 00164 { 00165 BOOST_FOREACH( std::string joint_name, joints ) 00166 { 00167 joint_command_publishers[joint_name] = (nh.advertise<std_msgs::Float64>("/" + joint_name + "/command", 1, false)); 00168 joint_relax_clients[joint_name] = (nh.serviceClient<arbotix_msgs::Relax>("/" + joint_name + "/relax")); 00169 } 00170 00171 } 00172 00173 void processArmFeedback(const InteractiveMarkerFeedbackConstPtr &feedback) 00174 { 00175 if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN && feedback->marker_name == "arm_marker") 00176 { 00177 arm_timer.stop(); 00178 } 00179 if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP || feedback->marker_name != "arm_marker") 00180 return; 00181 00182 if (immediate_commands) 00183 { 00184 sendTrajectoryCommand(feedback); 00185 } 00186 } 00187 00188 void processJointFeedback(const InteractiveMarkerFeedbackConstPtr &feedback) 00189 { 00190 std_msgs::Float64 command; 00191 command.data = feedback->pose.orientation.y; 00192 joint_command_publishers[feedback->marker_name].publish(command); 00193 } 00194 00195 void changeMarkerColor(double r, double g, double b, bool set_pose=false, geometry_msgs::Pose pose = geometry_msgs::Pose()) 00196 { 00197 InteractiveMarker int_marker; 00198 server.get("arm_marker", int_marker); 00199 00200 Marker *box_marker = &int_marker.controls[0].markers[0]; 00201 00202 box_marker->color.r = r; 00203 box_marker->color.g = g; 00204 box_marker->color.b = b; 00205 00206 if (set_pose) 00207 int_marker.pose = pose; 00208 00209 server.insert(int_marker); 00210 server.applyChanges(); 00211 } 00212 00213 void processCommand(const actionlib::SimpleClientGoalState& state, 00214 const simple_arm_server::MoveArmResultConstPtr& result, 00215 const InteractiveMarkerFeedbackConstPtr &feedback) 00216 { 00217 ROS_INFO("Finished in state [%s]", state.toString().c_str()); 00218 00219 //changeMarkerColor(0, 0, 1); 00220 if (state == actionlib::SimpleClientGoalState::SUCCEEDED) 00221 { 00222 changeMarkerColor(0, 1, 0, true, feedback->pose); 00223 ros::Duration(0.25).sleep(); 00224 resetMarker(); 00225 //arm_timer.start(); 00226 } 00227 else 00228 { 00229 changeMarkerColor(1, 0, 0, true, feedback->pose); 00230 } 00231 } 00232 00233 bool sendTrajectoryCommand(const InteractiveMarkerFeedbackConstPtr &feedback) 00234 { 00235 simple_arm_server::MoveArmGoal goal; 00236 simple_arm_server::ArmAction action; 00237 00238 goal.header.frame_id = root_link; 00239 00240 geometry_msgs::Pose pose; 00241 getTransformedPose(feedback->header.frame_id, feedback->pose, root_link, pose, feedback->header.stamp); 00242 00243 action.goal.orientation = pose.orientation; 00244 action.goal.position = pose.position; 00245 action.move_time = ros::Duration(move_time); 00246 goal.motions.push_back(action); 00247 00248 client.sendGoal(goal, boost::bind(&TurtlebotArmMarkerServer::processCommand, this, _1, _2, feedback)); 00249 changeMarkerColor(0, 0, 1, true, feedback->pose); 00250 00251 return true; 00252 } 00253 00254 bool sendGripperCommand(const InteractiveMarkerFeedbackConstPtr &feedback) 00255 { 00256 simple_arm_server::MoveArmGoal goal; 00257 simple_arm_server::ArmAction action; 00258 00259 goal.header.frame_id = tip_link; 00260 action.type = simple_arm_server::ArmAction::MOVE_GRIPPER; 00261 action.command = -feedback->pose.position.y; 00262 goal.motions.push_back(action); 00263 00264 client.sendGoal(goal); 00265 //client.waitForResult(ros::Duration(30.0)); 00266 //if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 00267 // return true; 00268 //return false; 00269 00270 // Don't block. Just return. 00271 return true; 00272 } 00273 00274 void getTransformedPose(const string& source_frame, const geometry_msgs::Pose& source_pose, 00275 const string& target_frame, geometry_msgs::Pose& target_pose, 00276 const ros::Time& time) 00277 { 00278 tf::Pose bt_source_pose; 00279 00280 tf::poseMsgToTF(source_pose, bt_source_pose); 00281 00282 tf::Stamped<tf::Pose> posein(bt_source_pose, time, source_frame); 00283 tf::Stamped<tf::Pose> poseout; 00284 00285 try 00286 { 00287 ros::Duration timeout(10.0); 00288 00289 // Get base_link transform 00290 tf_listener.waitForTransform(target_frame, source_frame, 00291 time, timeout); 00292 tf_listener.transformPose(target_frame, posein, poseout); 00293 00294 00295 } 00296 catch (tf::TransformException& ex) { 00297 ROS_WARN("[arm interactive markers] TF exception:\n%s", ex.what()); 00298 return; 00299 } 00300 00301 tf::poseTFToMsg(poseout, target_pose); 00302 } 00303 00304 void resetMarker() 00305 { 00306 geometry_msgs::Pose arm_pose; 00307 geometry_msgs::Pose blank_pose; 00308 blank_pose.orientation.w = 1; 00309 00310 getTransformedPose(tip_link, blank_pose, root_link, arm_pose, ros::Time(0)); 00311 00312 server.setPose("arm_marker", arm_pose); 00313 server.applyChanges(); 00314 } 00315 00316 void createArmMarker() 00317 { 00318 // Create a marker representing the tip of the arm. 00319 InteractiveMarker int_marker; 00320 int_marker.header.frame_id = root_link; 00321 int_marker.name = "arm_marker"; 00322 int_marker.scale = 0.1; 00323 00324 Marker marker; 00325 00326 marker.type = Marker::CUBE; 00327 marker.scale.x = 0.03; 00328 marker.scale.y = 0.03; 00329 marker.scale.z = 0.03; 00330 marker.color.r = 0.0; 00331 marker.color.g = 1.0; 00332 marker.color.b = 0.0; 00333 marker.color.a = .7; 00334 00335 InteractiveMarkerControl box_control; 00336 box_control.always_visible = true; 00337 box_control.interaction_mode = InteractiveMarkerControl::BUTTON; 00338 box_control.markers.push_back( marker ); 00339 int_marker.controls.push_back( box_control ); 00340 00341 InteractiveMarkerControl control; 00342 00343 control.orientation.w = 1; 00344 control.orientation.x = 1; 00345 control.orientation.y = 0; 00346 control.orientation.z = 0; 00347 control.name = "rotate_x"; 00348 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 00349 int_marker.controls.push_back(control); 00350 control.name = "move_x"; 00351 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 00352 int_marker.controls.push_back(control); 00353 00354 control.orientation.w = 1; 00355 control.orientation.x = 0; 00356 control.orientation.y = 1; 00357 control.orientation.z = 0; 00358 control.name = "rotate_z"; 00359 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 00360 int_marker.controls.push_back(control); 00361 control.name = "move_z"; 00362 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 00363 int_marker.controls.push_back(control); 00364 00365 control.orientation.w = 1; 00366 control.orientation.x = 0; 00367 control.orientation.y = 0; 00368 control.orientation.z = 1; 00369 control.name = "rotate_y"; 00370 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 00371 int_marker.controls.push_back(control); 00372 control.name = "move_y"; 00373 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 00374 int_marker.controls.push_back(control); 00375 00376 00377 server.insert(int_marker, boost::bind( &TurtlebotArmMarkerServer::processArmFeedback, this, _1 )); 00378 00379 server.applyChanges(); 00380 } 00381 00382 void createGripperMarker() 00383 { 00384 // Create a marker for the gripper 00385 InteractiveMarker int_marker; 00386 int_marker.header.frame_id = tip_link; 00387 int_marker.name = "gripper_marker"; 00388 int_marker.scale = 0.075; 00389 00390 int_marker.pose.position.x = gripper_marker_offset_x; 00391 int_marker.pose.position.y = gripper_marker_offset_y; 00392 int_marker.pose.position.z = gripper_marker_offset_z; 00393 00394 Marker marker; 00395 00396 marker.type = Marker::CUBE; 00397 marker.scale.x = 0.05; 00398 marker.scale.y = 0.005; 00399 marker.scale.z = 0.05; 00400 marker.color.r = 0.0; 00401 marker.color.g = 1.0; 00402 marker.color.b = 0.0; 00403 marker.color.a = .7; 00404 00405 marker.pose.position.x = gripper_box_offset_x; 00406 marker.pose.position.y = gripper_box_offset_y; 00407 marker.pose.position.z = gripper_box_offset_z; 00408 00409 InteractiveMarkerControl box_control; 00410 box_control.always_visible = true; 00411 box_control.interaction_mode = InteractiveMarkerControl::BUTTON; 00412 box_control.markers.push_back( marker ); 00413 int_marker.controls.push_back( box_control ); 00414 00415 InteractiveMarkerControl control; 00416 00417 control.orientation.w = 1; 00418 control.orientation.x = 0; 00419 control.orientation.y = 0; 00420 control.orientation.z = 1; 00421 control.name = "move_z"; 00422 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; 00423 int_marker.controls.push_back(control); 00424 00425 server.insert(int_marker, boost::bind( &TurtlebotArmMarkerServer::sendGripperCommand, this, _1 )); 00426 00427 server.applyChanges(); 00428 } 00429 00430 void createJointMarkers() 00431 { 00432 for (unsigned int i = 0; i < joints.size() && i < links.size(); i++) 00433 { 00434 createJointMarker(joints[i], links[i]); 00435 } 00436 } 00437 00438 void createJointMarker(const string joint_name, const string link_name) 00439 { 00440 InteractiveMarker int_marker; 00441 int_marker.header.frame_id = link_name; 00442 int_marker.name = joint_name; 00443 int_marker.scale = 0.05; 00444 00445 InteractiveMarkerControl control; 00446 00447 control.orientation.w = 1; 00448 control.orientation.x = 0; 00449 control.orientation.y = 0; 00450 control.orientation.z = 1; 00451 control.name = "rotate_z"; 00452 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; 00453 int_marker.controls.push_back(control); 00454 00455 server.insert(int_marker, boost::bind( &TurtlebotArmMarkerServer::processJointFeedback, this, _1 )); 00456 00457 server.applyChanges(); 00458 } 00459 00460 void createArmMenu() 00461 { 00462 arm_menu_handler = MenuHandler(); 00463 arm_menu_handler.insert("Send command", boost::bind(&TurtlebotArmMarkerServer::sendCommandCb, this, _1)); 00464 arm_menu_handler.setCheckState( 00465 arm_menu_handler.insert("Send command immediately", boost::bind(&TurtlebotArmMarkerServer::immediateCb, this, _1)), MenuHandler::CHECKED ); 00466 arm_menu_handler.insert("Reset marker", boost::bind(&TurtlebotArmMarkerServer::resetPoseCb, this, _1)); 00467 00468 MenuHandler::EntryHandle entry = arm_menu_handler.insert("Relax joints"); 00469 arm_menu_handler.insert( entry, "All", boost::bind(&TurtlebotArmMarkerServer::relaxAllCb, this, _1)); 00470 BOOST_FOREACH(string joint_name, joints) 00471 { 00472 arm_menu_handler.insert( entry, joint_name, boost::bind(&TurtlebotArmMarkerServer::relaxCb, this, joint_name, _1) ); 00473 } 00474 00475 arm_menu_handler.insert("Switch to Joint Control", boost::bind(&TurtlebotArmMarkerServer::switchToJointControlCb, this, _1)); 00476 00477 arm_menu_handler.apply( server, "arm_marker" ); 00478 server.applyChanges(); 00479 } 00480 00481 void createJointMenus() 00482 { 00483 BOOST_FOREACH(string joint_name, joints) 00484 { 00485 createJointMenu(joint_name); 00486 } 00487 } 00488 00489 void createJointMenu(const string joint_name) 00490 { 00491 MenuHandler joint_handler; 00492 joint_handler.insert("Relax all", boost::bind(&TurtlebotArmMarkerServer::relaxAllCb, this, _1)); 00493 joint_handler.insert("Relax this joint", boost::bind(&TurtlebotArmMarkerServer::relaxCb, this, joint_name, _1)); 00494 joint_handler.insert("Switch to Trajectory Control", boost::bind(&TurtlebotArmMarkerServer::switchToArmControlCb, this, _1)); 00495 00496 joint_menu_handlers[joint_name] = joint_handler; 00497 00498 joint_menu_handlers[joint_name].apply( server, joint_name ); 00499 server.applyChanges(); 00500 } 00501 00502 void sendCommandCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 00503 { 00504 sendTrajectoryCommand(feedback); 00505 } 00506 00507 void relaxAllCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 00508 { 00509 BOOST_FOREACH( std::string joint_name, joints ) 00510 { 00511 relaxCb(joint_name, feedback); 00512 } 00513 00514 // Do this twice to make sure they're all relaxed 00515 BOOST_FOREACH( std::string joint_name, joints ) 00516 { 00517 relaxCb(joint_name, feedback); 00518 } 00519 } 00520 00521 void relaxCb(const std::string joint_name, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 00522 { 00523 arbotix_msgs::Relax srv; 00524 joint_relax_clients[joint_name].call(srv); 00525 } 00526 00527 void immediateCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 00528 { 00529 MenuHandler::EntryHandle handle = feedback->menu_entry_id; 00530 MenuHandler::CheckState state; 00531 arm_menu_handler.getCheckState( handle, state ); 00532 00533 if ( state == MenuHandler::CHECKED ) 00534 { 00535 arm_menu_handler.setCheckState( handle, MenuHandler::UNCHECKED ); 00536 ROS_INFO("Switching to cached commands"); 00537 immediate_commands = false; 00538 } 00539 else 00540 { 00541 arm_menu_handler.setCheckState( handle, MenuHandler::CHECKED ); 00542 ROS_INFO("Switching to immediate comands"); 00543 immediate_commands = true; 00544 } 00545 arm_menu_handler.reApply(server); 00546 00547 server.applyChanges(); 00548 } 00549 00550 void resetPoseCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 00551 { 00552 resetMarker(); 00553 arm_timer.start(); 00554 } 00555 00556 void switchToJointControlCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 00557 { 00558 server.clear(); 00559 server.applyChanges(); 00560 00561 createJointMarkers(); 00562 createJointMenus(); 00563 } 00564 00565 void switchToArmControlCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 00566 { 00567 server.clear(); 00568 server.applyChanges(); 00569 00570 createArmMarker(); 00571 createGripperMarker(); 00572 //createArmMenu(); 00573 00574 arm_menu_handler.reApply(server); 00575 00576 resetMarker(); 00577 } 00578 }; 00579 00580 int main(int argc, char** argv) 00581 { 00582 ros::init(argc, argv, "turtlebot_arm_marker_server"); 00583 TurtlebotArmMarkerServer turtleserver; 00584 00585 ros::spin(); 00586 }