00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00071 vector<string> joints;
00072 vector<string> links;
00073 string tip_link;
00074 string root_link;
00075
00076
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
00086 double move_time;
00087
00088
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
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
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
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
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
00220 if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
00221 {
00222 changeMarkerColor(0, 1, 0, true, feedback->pose);
00223 ros::Duration(0.25).sleep();
00224 resetMarker();
00225
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
00266
00267
00268
00269
00270
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
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
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
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
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
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 }