coverage_3d_arm_navigation: CollisionCheck.cpp Source File

Go to the documentation of this file.
00001 
00002 /*
00003  * CollisionCheck.cpp
00004  *
00005  *  Created on: Jan 18, 2012
00006  *      Author: hess
00007  */
00008 
00009 #include <coverage_3d_arm_navigation/CollisionCheck.h>
00010 
00011 #include <arm_navigation_msgs/GetPlanningScene.h>
00012 #include <planning_environment/models/collision_models.h>
00013 
00014 CollisionCheck::CollisionCheck(ros::NodeHandle& nh) :
00015                 nh_(nh), planning_scene_avail_(false), collision_models_(0), state_(0) {
00016 
00017         vis_marker_array_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>("robot_state_validity", 132);
00018         collision_models_ = new planning_environment::CollisionModels("/robot_description");
00019         right_arm_names = collision_models_->getKinematicModel()->getModelGroup("right_arm")->getUpdatedLinkModelNames();
00020         right_joint_names = collision_models_->getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();
00021         left_arm_names = collision_models_->getKinematicModel()->getModelGroup("left_arm")->getUpdatedLinkModelNames();
00022         left_joint_names = collision_models_->getKinematicModel()->getModelGroup("left_arm")->getJointModelNames();
00023 
00024         std::string PLANNING_SCENE_SRV = "/environment_server/set_planning_scene_diff";
00025         ros::service::waitForService(PLANNING_SCENE_SRV);
00026         get_planning_scene_client_ = nh_.serviceClient<arm_navigation_msgs::GetPlanningScene>(PLANNING_SCENE_SRV);
00027 }
00028 
00029 CollisionCheck::~CollisionCheck() {
00030         collision_models_->revertPlanningScene(state_); //this is a very weird planning scene construct --> you must delete pointer this way, no delete statement
00031 }
00032 bool CollisionCheck::requestPlanningScene() {
00033 
00034         planning_scene_avail_ = false;
00035 
00036         arm_navigation_msgs::AttachedCollisionObject att_object;
00037         att_object.link_name = "r_gripper_palm_link";
00038         att_object.object.id = "attached_cylinder";
00039         att_object.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00040         att_object.object.header.frame_id = "r_gripper_palm_link";
00041         att_object.object.header.stamp = ros::Time::now();
00042         arm_navigation_msgs::Shape object;
00043         object.type = arm_navigation_msgs::Shape::CYLINDER;
00044         object.dimensions.resize(2);
00045         object.dimensions[0] = 0.06;
00046         object.dimensions[1] = 0.08;
00047         geometry_msgs::Pose pose;
00048         pose.position.x = 0.12;
00049         pose.position.y = 0.0;
00050         pose.position.z = 0.0;
00051         pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, M_PI / 2.0, 0);
00052         att_object.object.shapes.push_back(object);
00053         att_object.object.poses.push_back(pose);
00054         att_object.touch_links.push_back("r_end_effector");
00055 
00056         arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
00057 //      planning_scene_req.planning_scene_diff.attached_collision_objects.push_back(att_object);
00058 
00059         arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
00060         if (!get_planning_scene_client_.call(planning_scene_req, planning_scene_res)) {
00061                 ROS_WARN("Can't get planning scene");
00062                 return false;
00063         }
00064         get_planning_scene_client_.shutdown();
00065         scene_ = planning_scene_res.planning_scene;
00066         state_ = collision_models_->setPlanningScene(scene_);
00067 
00068         // do not check the fingers for collisions
00069         collision_space::EnvironmentModel::AllowedCollisionMatrix new_coll_matrix =
00070                         collision_space::EnvironmentModel::AllowedCollisionMatrix(
00071                                         collision_models_->getDefaultAllowedCollisionMatrix());
00072         new_coll_matrix.changeEntry("r_gripper_r_finger_tip_link", "collision_map", true);
00073         new_coll_matrix.changeEntry("collision_map", "r_gripper_r_finger_tip_link", true);
00074         new_coll_matrix.changeEntry("r_gripper_l_finger_tip_link", "collision_map", true);
00075         new_coll_matrix.changeEntry("collision_map", "r_gripper_l_finger_tip_link", true);
00076         new_coll_matrix.changeEntry("l_gripper_r_finger_tip_link", "collision_map", true);
00077         new_coll_matrix.changeEntry("collision_map", "l_gripper_r_finger_tip_link", true);
00078         new_coll_matrix.changeEntry("l_gripper_l_finger_tip_link", "collision_map", true);
00079         new_coll_matrix.changeEntry("collision_map", "l_gripper_l_finger_tip_link", true);
00080 
00081 //      new_coll_matrix.changeEntry("attached_cylinder", "r_wrist_roll_link", true);
00082 //      new_coll_matrix.changeEntry("r_wrist_roll_link", "attached_cylinder", true);
00083 //      new_coll_matrix.changeEntry("attached_cylinder", "r_gripper_r_finger_link", true);
00084 //      new_coll_matrix.changeEntry("r_gripper_r_finger_link", "attached_cylinder", true);
00085 //      new_coll_matrix.changeEntry("attached_cylinder", "r_gripper_l_finger_link", true);
00086 //      new_coll_matrix.changeEntry("r_gripper_l_finger_link", "attached_cylinder", true);
00087 //      new_coll_matrix.changeEntry("attached_cylinder", "r_gripper_palm_link", true);
00088 //      new_coll_matrix.changeEntry("r_gripper_palm_link", "attached_cylinder", true);
00089         collision_models_->setAlteredAllowedCollisionMatrix(new_coll_matrix);
00090 
00091         planning_scene_avail_ = true;
00092         return true;
00093 }
00094 
00095 bool CollisionCheck::checkJointStateRight(const std::vector<double>& arm_state) {
00096         if (!planning_scene_avail_) {
00097                 requestPlanningScene();
00098         }
00099 
00100         return checkArmJointState(arm_state, "right_arm");
00101 }
00102 
00103 bool CollisionCheck::checkJointStateLeft(const std::vector<double>& arm_state) {
00104         if (!planning_scene_avail_) {
00105                 requestPlanningScene();
00106         }
00107         return checkArmJointState(arm_state, "left_arm");
00108 }
00109 
00110 bool CollisionCheck::checkArmJointState(const std::vector<double>& arm_state, const std::string arm) {
00111 
00112         ROS_ASSERT_MSG(arm_state.size()==7, "The joint state of the arm does not have the correct size.");
00113 
00114         bool visualize = false;
00115 
00116         std::vector<std::string> arm_names;
00117         std::vector<std::string> joint_names;
00118         std::map<std::string, double> nvalues;
00119 
00120 //      collision_models_->disableCollisionsForNonUpdatedLinks("right_arm");
00121         if (arm.compare("right_arm") == 0) {
00122                 ROS_DEBUG_NAMED("CollisionCheck", "Checking collision for right arm.");
00123                 arm_names = right_arm_names;
00124                 joint_names = right_joint_names;
00125 
00126                 //set kinematic state into the right form
00127                 nvalues["r_shoulder_pan_joint"] = arm_state[0];
00128                 nvalues["r_shoulder_lift_joint"] = arm_state[1];
00129                 nvalues["r_upper_arm_roll_joint"] = arm_state[2];
00130                 nvalues["r_elbow_flex_joint"] = arm_state[3];
00131                 nvalues["r_forearm_roll_joint"] = arm_state[4];
00132                 nvalues["r_wrist_flex_joint"] = arm_state[5];
00133                 nvalues["r_wrist_roll_joint"] = arm_state[6];
00134         } else if (arm.compare("left_arm") == 0) {
00135                 ROS_DEBUG_NAMED("CollisionCheck", "Checking collision for left arm.");
00136                 arm_names = left_arm_names;
00137                 joint_names = left_joint_names;
00138 
00139                 //set kinematic state into the right form
00140                 nvalues["l_shoulder_pan_joint"] = arm_state[0];
00141                 nvalues["l_shoulder_lift_joint"] = arm_state[1];
00142                 nvalues["l_upper_arm_roll_joint"] = arm_state[2];
00143                 nvalues["l_elbow_flex_joint"] = arm_state[3];
00144                 nvalues["l_forearm_roll_joint"] = arm_state[4];
00145                 nvalues["l_wrist_flex_joint"] = arm_state[5];
00146                 nvalues["l_wrist_roll_joint"] = arm_state[6];
00147         }
00148 
00149         state_->setKinematicState(nvalues);
00150 
00151         visualize = false;
00152 
00153         std_msgs::ColorRGBA good_color, collision_color, joint_limits_color;
00154         good_color.a = collision_color.a = joint_limits_color.a = .8;
00155         good_color.g = 1.0;
00156         collision_color.r = 1.0;
00157         joint_limits_color.b = 1.0;
00158 
00159         std_msgs::ColorRGBA point_markers;
00160         point_markers.a = 1.0;
00161         point_markers.r = 1.0;
00162         point_markers.g = .8;
00163 
00164         std_msgs::ColorRGBA color;
00165         color = good_color;
00166         bool state_okay = true;
00167 
00168         visualization_msgs::MarkerArray marker_array;
00169         if (!state_->areJointsWithinBounds(joint_names)) {
00170 //              ROS_INFO_NAMED("CollisionCheck", "joints not in bounds");
00171                 state_okay = false;
00172                 color = joint_limits_color;
00173         }
00174 
00175 //      else if (collision_models_->isKinematicStateInCollision(*state_)) { TODO: comment in collision check
00176 //              state_okay = false;
00177 //              color = collision_color;
00178 //              std::vector<arm_navigation_msgs::ContactInformation> contacts;
00179 //              collision_models_->getAllCollisionsForState(*state_, contacts, 1);
00180 //              if ((!state_okay) && (visualize)) {
00181 //                      collision_models_->getAllCollisionPointMarkers(*state_, marker_array, point_markers, ros::Duration(0.2));
00182 //              }
00183 //
00184 //      }
00185 //      if (state_okay) {
00186 //              std::cout << "state is okay " << std::endl;
00187 //      }
00188         if (visualize) {
00189                 collision_models_->getAttachedCollisionObjectMarkers(*state_, marker_array, arm, color, ros::Duration(0.2),
00190                                 false, &arm_names);
00191                 collision_models_->getRobotMarkersGivenState(*state_, marker_array, color, arm, ros::Duration(0.2), &arm_names);
00192                 vis_marker_array_publisher_.publish(marker_array);
00193                 ros::Duration(0.2).sleep();
00194         }
00195 
00196         return state_okay;
00197 }