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
00031
00032
00033
00034
00047 #include <string>
00048 #include <map>
00049 #include <vector>
00050
00051 #include <geometry_msgs/PoseStamped.h>
00052 #include <geometry_msgs/PoseArray.h>
00053 #include <geometry_msgs/TransformStamped.h>
00054 #include <kinematics_msgs/GetPositionFK.h>
00055 #include <ros/ros.h>
00056 #include <sensor_msgs/JointState.h>
00057 #include <tf/transform_listener.h>
00058 #include <tf/transform_broadcaster.h>
00059 #include <tree_kinematics/get_tree_position_ik.h>
00060 #include <arm_navigation_msgs/GetPlanningScene.h>
00061 #include <planning_environment/models/collision_models.h>
00062
00063 #include <trajectory_msgs/JointTrajectory.h>
00064
00065
00066
00067
00068
00069 static const std::string FK_SERVICE = "/tree_kinematics_node/get_position_fk";
00070 static const std::string IK_SERVICE = "/tree_kinematics_node/get_position_ik";
00071 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
00072 static const std::string PUB_TOPIC_JOINT_STATES_CMD = "/joint_states_cmd";
00073 static const std::string SUB_TOPIC_JOINT_STATES = "/joint_states";
00074
00075 bool joint_states_valid = false;
00076 sensor_msgs::JointState old_joint_state;
00077
00078
00079 sensor_msgs::JointState::ConstPtr joint_states_ptr;
00080
00091 void jointStatesCB(const sensor_msgs::JointState::ConstPtr& joint_states)
00092 {
00093 joint_states_ptr = joint_states;
00094 if(!joint_states_valid)
00095 old_joint_state = *joint_states;
00096 joint_states_valid = true;
00097 }
00098
00099
00113 bool getGoalTransform(tf::TransformListener& tf_listener,
00114 std::string& root_frame_name,
00115 std::string goal_frame_name,
00116 geometry_msgs::PoseStamped& pose)
00117 {
00118 tf::StampedTransform transform;
00119 try
00120 {
00121 tf_listener.waitForTransform(root_frame_name, goal_frame_name, ros::Time(0), ros::Duration(0.5));
00122 tf_listener.lookupTransform(root_frame_name, goal_frame_name, ros::Time(0), transform);
00123 }
00124 catch (tf::TransformException const &ex)
00125 {
00126 ROS_DEBUG("%s",ex.what());
00127 ROS_WARN("No transformation available from '%s' to '%s'", root_frame_name.c_str(), goal_frame_name.c_str());
00128 return false;
00129 }
00130 pose.header.stamp = ros::Time::now();
00131 pose.header.frame_id = root_frame_name;
00132 pose.pose.position.x = transform.getOrigin().x();
00133 pose.pose.position.y = transform.getOrigin().y();
00134 pose.pose.position.z = transform.getOrigin().z();
00135 pose.pose.orientation.x = transform.getRotation().x();
00136 pose.pose.orientation.y = transform.getRotation().y();
00137 pose.pose.orientation.z = transform.getRotation().z();
00138 pose.pose.orientation.w = transform.getRotation().w();
00139
00140 return true;
00141 }
00142
00143
00144 int main(int argc, char** argv)
00145 {
00146 using namespace arm_navigation_msgs;
00147
00148 ros::init(argc, argv, "reem_teleop_coordinator_node");
00149 ros::NodeHandle nh, nh_private("~");
00150
00151 int nr_of_endpts = 0;
00152 std::vector<std::string> endpts;
00153 endpts.clear();
00154 if(nh_private.getParam("nr_of_endpoints", nr_of_endpts) && nr_of_endpts >= 0)
00155 {
00156 for(int i = 0; i < nr_of_endpts; ++i)
00157 {
00158 std::string elementname;
00159 std::string endpt_name;
00160 std::stringstream ss;
00161 ss << "endpoint_" << i;
00162 ss >> elementname;
00163 if(nh_private.getParam(elementname, endpt_name))
00164 {
00165 endpts.push_back(endpt_name);
00166 ROS_INFO("Added endpoint '%s'", endpts[i].c_str());
00167 }
00168 else
00169 {
00170 ROS_FATAL("Couldn't get the name of an endpoint! Aborting...");
00171 return 0;
00172 }
00173 }
00174 }
00175
00176 std::vector<std::string> comand_names;
00177
00178 geometry_msgs::PoseStamped pose;
00179 std::map<std::string, geometry_msgs::PoseStamped> poses;
00180 std::map<std::string, std::string> rel_endpoints_goals;
00181 for(unsigned int i = 0; i < endpts.size(); ++i)
00182 {
00183 std::string elementname;
00184 std::string endpt_goal_name;
00185 std::stringstream ss;
00186 ss << "endpoint_" << i << "_goal";
00187 ss >> elementname;
00188 if(nh_private.getParam(elementname, endpt_goal_name))
00189 {
00190 poses.insert(std::map<std::string, geometry_msgs::PoseStamped>::value_type(endpt_goal_name, pose));
00191 rel_endpoints_goals.insert(std::map<std::string, std::string>::value_type(endpt_goal_name, endpts[i]));
00192 ROS_INFO("Added endpoint goal '%s'", endpt_goal_name.c_str());
00193 ROS_INFO("Goal '%s' is linked to endpoint '%s'", endpt_goal_name.c_str(), endpts[i].c_str());
00194 }
00195 else
00196 {
00197 ROS_FATAL("Couldn't get the name of an endpoint goal! Aborting...");
00198 return 0;
00199 }
00200 }
00201
00202 std::string root_frame_name;
00203 if(nh_private.getParam("root_frame_name", root_frame_name))
00204 ROS_INFO("root frame name: %s", root_frame_name.c_str());
00205 else
00206 {
00207 ROS_FATAL("Couldn't get the name of the root frame! Aborting...");
00208 return 0;
00209 }
00210
00211 tf::TransformListener tf_listener;
00212 tf::TransformBroadcaster tf_broadcaster;
00213 geometry_msgs::TransformStamped transform;
00214
00215
00216 ros::Subscriber sub_joint_states = nh.subscribe(SUB_TOPIC_JOINT_STATES, 20, jointStatesCB);
00217
00218
00219 tree_kinematics::get_tree_position_ik tree_ik_srv;
00220
00221
00222
00223
00224 kinematics_msgs::GetPositionFK tree_fk_srv;
00225
00226
00227
00228
00229 bool no_self_collision = false;
00230 bool check_self_collision, check_joint_limits = true;
00231 nh_private.param("check_self_collision", check_self_collision, true);
00232 nh_private.param("check_joint_limits", check_joint_limits, true);
00233 ROS_INFO("checking for self-collision: %s, checking for joint limits: %s", (check_self_collision)?"true":"false",
00234 (check_joint_limits)?"true":"false");
00235
00236
00237 ros::Publisher pub_joint_states_cmd = nh.advertise<sensor_msgs::JointState>(PUB_TOPIC_JOINT_STATES_CMD, 1);
00238
00239 ros::Publisher traj_publisher_ = nh.advertise<trajectory_msgs::JointTrajectory>("/upper_body_controller/command", 1);
00240
00241 sensor_msgs::JointState joint_states_cmd;
00242
00243
00244
00245
00246
00247
00248
00249
00250 int loop_rate_value;
00251 nh_private.param("loop_rate", loop_rate_value, 10);
00252 ROS_INFO("loop rate: %d", loop_rate_value);
00253 ros::Rate loop_rate(loop_rate_value);
00254 unsigned int loop_count = 1;
00255 double cycle_time_median = 0.0;
00256 double ik_duration = 0.0;
00257 double ik_duration_median = 0.0;
00258 double scc_duration = 0.0;
00259 double scc_duration_median = 0.0;
00260 double cjp_duration = 0.0;
00261 double cjp_duration_median = 0.0;
00262
00263 ros::service::waitForService(IK_SERVICE);
00264 ros::ServiceClient tree_ik_srv_client = nh.serviceClient<tree_kinematics::get_tree_position_ik> (IK_SERVICE, true);
00265
00266 ros::service::waitForService(FK_SERVICE);
00267 ros::ServiceClient tree_fk_srv_client = nh.serviceClient<kinematics_msgs::GetPositionFK> (FK_SERVICE, true);
00268
00269 ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
00270 ros::ServiceClient get_planning_scene_client = nh.serviceClient<GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME, true);
00271 planning_environment::CollisionModels collision_models("robot_description");
00272 collision_models.disableCollisionsForNonUpdatedLinks("upper_body");
00273 std::vector<std::string> joint_names = collision_models.getKinematicModel()->getModelGroup("upper_body")->getJointModelNames();
00274 while (nh.ok())
00275 {
00276 ros::spinOnce();
00277
00278
00279 std::map<std::string, geometry_msgs::PoseStamped>::iterator poses_it;
00280 geometry_msgs::PoseStamped pose;
00281 bool goal_transforms_valid = true;
00282 for(poses_it = poses.begin(); poses_it != poses.end(); ++poses_it)
00283 {
00284 if(getGoalTransform(tf_listener, root_frame_name, poses_it->first, pose))
00285 poses_it->second = pose;
00286 else
00287 {
00288 ROS_WARN_THROTTLE(0.5, "No valid transformation available for endpoint goal '%s'", poses_it->first.c_str());
00289 ROS_WARN_THROTTLE(0.5, "No commands will be calculated.");
00290 goal_transforms_valid = false;
00291 }
00292 }
00293
00294 if (joint_states_valid && goal_transforms_valid)
00295 {
00296
00297
00298
00299 tree_ik_srv.request.pos_ik_request.clear();
00300 kinematics_msgs::PositionIKRequest pos_ik_request;
00301 std::map<std::string, std::string>::iterator rel_it;
00302 for(poses_it = poses.begin(); poses_it != poses.end(); ++poses_it)
00303 {
00304 rel_it = rel_endpoints_goals.find(poses_it->first);
00305 pos_ik_request.ik_link_name = rel_it->second;
00306 pos_ik_request.pose_stamped = poses_it->second;
00307 tree_ik_srv.request.pos_ik_request.push_back(pos_ik_request);
00308 }
00309
00310
00311 if (joint_states_ptr)
00312
00313 tree_ik_srv.request.pos_ik_request[0].ik_seed_state.joint_state = old_joint_state;
00314 else
00315 {
00316 ROS_ERROR("joint_states_ptr invalid! Aborting loop ...");
00317 continue;
00318 }
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341 ik_duration = ros::Time::now().toSec();
00342 if (tree_ik_srv_client.call(tree_ik_srv))
00343 {
00344 for(unsigned int i = 0; i < tree_ik_srv.response.solution.joint_state.name.size(); ++i)
00345 {
00346 ROS_DEBUG_THROTTLE(1.0, "desired position for joint[%d]('%s'): %f", i,
00347 tree_ik_srv.response.solution.joint_state.name[i].c_str(),
00348 tree_ik_srv.response.solution.joint_state.position[i]);
00349 }
00350 }
00351 else
00352 {
00353 ROS_ERROR_THROTTLE(1.0, "get_tree_position_ik service call failed! Aborting loop ...");
00354 continue;
00355 }
00356 ik_duration = ros::Time::now().toSec() - ik_duration;
00357 ik_duration_median = ((ik_duration_median * (loop_count - 1)) + ik_duration) / loop_count;
00358
00359
00360 GetPlanningScene::Request planning_scene_req;
00361 GetPlanningScene::Response planning_scene_res;
00362 if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res))
00363 {
00364 ROS_WARN("Can't get planning scene");
00365 continue;
00366 }
00367 planning_models::KinematicState* state = collision_models.setPlanningScene(planning_scene_res.planning_scene);
00368
00369 std::map<std::string, double> nvalues;
00370 const sensor_msgs::JointState& sol_state = tree_ik_srv.response.solution.joint_state;
00371 for (size_t ith_joint = 0; ith_joint < sol_state.name.size(); ++ith_joint)
00372 {
00373 nvalues[sol_state.name[ith_joint]] = sol_state.position[ith_joint];
00374 }
00375 state->setKinematicState(nvalues);
00376 if (check_joint_limits && !state->areJointsWithinBounds(joint_names))
00377 {
00378 ROS_WARN_THROTTLE(1.0, "Requested state violates joint limits.");
00379 }
00380 else if (check_self_collision && collision_models.isKinematicStateInCollision(*state))
00381 {
00382 ROS_WARN_THROTTLE(1.0, "Requested state is in collision.");
00383 }
00384 else
00385 {
00386 ROS_DEBUG_THROTTLE(1.0, "Requested state is valid.");
00387 no_self_collision = true;
00388 }
00389 collision_models.revertPlanningScene(state);
00390
00391 scc_duration = ros::Time::now().toSec();
00392 scc_duration = ros::Time::now().toSec() - scc_duration;
00393 scc_duration_median = ((scc_duration_median * (loop_count - 1)) + scc_duration) / loop_count;
00394
00395
00396
00397 if(no_self_collision == true)
00398 {
00399 joint_states_cmd = tree_ik_srv.response.solution.joint_state;
00400 joint_states_cmd.header.stamp = ros::Time::now();
00401 pub_joint_states_cmd.publish(joint_states_cmd);
00402
00403 trajectory_msgs::JointTrajectory traj_;
00404 traj_.header.stamp = ros::Time::now();
00405
00406 traj_.joint_names = joint_states_cmd.name;
00407 traj_.points.resize(1);
00408 traj_.points[0].positions = joint_states_cmd.position;
00409 traj_.points[0].velocities = joint_states_cmd.velocity;
00410 traj_.points[0].time_from_start = ros::Duration(0.001);
00411 traj_publisher_.publish(traj_);
00412
00413 old_joint_state = joint_states_cmd;
00414 no_self_collision = false;
00415
00416 }
00417 else
00418 {
00419 for(unsigned int i = 0; i < old_joint_state.name.size(); ++i)
00420 old_joint_state.velocity[i] = 0.0;
00421 joint_states_cmd = old_joint_state;
00422 joint_states_cmd.header.stamp = ros::Time::now();
00423 pub_joint_states_cmd.publish(joint_states_cmd);
00424
00425 trajectory_msgs::JointTrajectory traj_;
00426 traj_.header.stamp = ros::Time::now();
00427 traj_.points.resize(1);
00428
00429 for(int k=0; k<joint_states_cmd.name.size(); ++k){
00430
00431 if(std::find(comand_names.begin(), comand_names.end(),joint_states_cmd.name[k] ) != comand_names.end()){
00432 traj_.joint_names.push_back(joint_states_cmd.name[k]);
00433 traj_.points[0].positions.push_back(joint_states_cmd.position[k]);
00434 traj_.points[0].velocities.push_back(joint_states_cmd.velocity[k]);
00435 }
00436 }
00437 traj_.points[0].time_from_start = ros::Duration(0.001);
00438 traj_publisher_.publish(traj_);
00439
00440 }
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490 }
00491
00492 ros::spinOnce();
00493
00494 loop_rate.sleep();
00495
00496 cycle_time_median = ((cycle_time_median * (loop_count - 1)) + loop_rate.cycleTime().toSec()) / loop_count;
00497
00498 ROS_DEBUG_THROTTLE(1.0, "reem_teleop: cycle time %f and median cycle time %f",
00499 loop_rate.cycleTime().toSec(), cycle_time_median);
00500 ROS_DEBUG_THROTTLE(1.0, "reem_teleop: IKC current duration %f and median %f", ik_duration, ik_duration_median);
00501 ROS_DEBUG_THROTTLE(1.0, "reem_teleop: SCC current duration %f and median %f", scc_duration, scc_duration_median);
00502 ROS_DEBUG_THROTTLE(1.0, "reem_teleop: FKC current duration %f and median %f", cjp_duration, cjp_duration_median);
00503
00504 loop_count ++;
00505 }
00506 tree_ik_srv_client.shutdown();
00507 get_planning_scene_client.shutdown();
00508 pub_joint_states_cmd.shutdown();
00509 tree_fk_srv_client.shutdown();
00510
00511 return 0;
00512 }
00513