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
00037 #include <ros/ros.h>
00038
00039 #include <planning_environment/models/collision_models.h>
00040 #include <planning_environment/monitors/collision_space_monitor.h>
00041 #include <planning_environment/models/model_utils.h>
00042
00043 #include <arm_navigation_msgs/GetStateValidity.h>
00044
00045 #include <visualization_msgs/MarkerArray.h>
00046 #include <visualization_msgs/Marker.h>
00047
00048 #include <tf/transform_listener.h>
00049
00050 static const ros::Duration MARKER_DUR(.2);
00051
00052 class CurrentStateValidator {
00053
00054 public:
00055
00056 CurrentStateValidator(void): priv_handle_("~")
00057 {
00058 std::string robot_description_name = root_handle_.resolveName("robot_description", true);
00059
00060 collision_models_ = new planning_environment::CollisionModels(robot_description_name);
00061 csm_ = new planning_environment::CollisionSpaceMonitor(collision_models_, &tf_);
00062
00063 csm_->waitForState();
00064 csm_->setUseCollisionMap(true);
00065 csm_->startEnvironmentMonitor();
00066
00067 priv_handle_.param<std::string>("group_name_1", group_name_1_, "");
00068 priv_handle_.param<std::string>("group_name_2", group_name_2_, "");
00069
00070 priv_handle_.param<bool>("send_markers", send_markers_, "false");
00071
00072 vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("current_state_validator", 128);
00073 vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("current_state_validator_array", 128);
00074
00075 csm_->addOnStateUpdateCallback(boost::bind(&CurrentStateValidator::jointStateCallback, this, _1));
00076
00077 get_state_validity_service_ = priv_handle_.advertiseService("get_state_validity", &CurrentStateValidator::getStateValidity, this);
00078 }
00079
00080 bool getStateValidity(arm_navigation_msgs::GetStateValidity::Request &req,
00081 arm_navigation_msgs::GetStateValidity::Response &res)
00082 {
00083 collision_models_->bodiesLock();
00084
00085 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = collision_models_->getDefaultAllowedCollisionMatrix();
00086
00087 if(!acm.hasEntry(COLLISION_MAP_NAME)) {
00088 acm.addEntry(COLLISION_MAP_NAME, false);
00089 collision_models_->setAlteredAllowedCollisionMatrix(acm);
00090 }
00091
00092 if(!req.group_name.empty()) {
00093 collision_models_->disableCollisionsForNonUpdatedLinks(req.group_name, true);
00094 }
00095
00096 planning_models::KinematicState state(collision_models_->getKinematicModel());
00097 csm_->setStateValuesFromCurrentValues(state);
00098
00099 planning_environment::setRobotStateAndComputeTransforms(req.robot_state,
00100 state);
00101 arm_navigation_msgs::Constraints goal_constraints;
00102 if(req.check_goal_constraints) {
00103 goal_constraints = req.goal_constraints;
00104 }
00105 arm_navigation_msgs::Constraints path_constraints;
00106 if(req.check_path_constraints) {
00107 path_constraints = req.path_constraints;
00108 }
00109 std::vector<std::string> joint_names;
00110 if(req.check_joint_limits) {
00111 joint_names = req.robot_state.joint_state.name;
00112 }
00113 collision_models_->isKinematicStateValid(state,
00114 joint_names,
00115 res.error_code,
00116 goal_constraints,
00117 path_constraints);
00118
00119 if(!req.group_name.empty()) {
00120 collision_models_->revertAllowedCollisionToDefault();
00121 }
00122
00123 collision_models_->bodiesUnlock();
00124 return true;
00125
00126 }
00127
00128 void sendMarkersForGroup(const std::string& group) {
00129
00130 collision_models_->bodiesLock();
00131
00132 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = collision_models_->getDefaultAllowedCollisionMatrix();
00133
00134 if(!acm.hasEntry(COLLISION_MAP_NAME)) {
00135 acm.addEntry(COLLISION_MAP_NAME, false);
00136 collision_models_->setAlteredAllowedCollisionMatrix(acm);
00137 }
00138
00139
00140
00141 if(!group.empty()) {
00142 collision_models_->disableCollisionsForNonUpdatedLinks(group, true);
00143 }
00144
00145 planning_models::KinematicState state(collision_models_->getKinematicModel());
00146 csm_->setStateValuesFromCurrentValues(state);
00147
00148 std_msgs::ColorRGBA good_color;
00149 good_color.a = 1.0;
00150 good_color.r = 0.1;
00151 good_color.g = 0.8;
00152 good_color.b = 0.3;
00153
00154 std_msgs::ColorRGBA bad_color;
00155 bad_color.a = 1.0;
00156 bad_color.r = 1.0;
00157 bad_color.g = 0.0;
00158 bad_color.b = 0.0;
00159
00160 std_msgs::ColorRGBA col;
00161 bool send = true;
00162 if(collision_models_->isKinematicStateInCollision(state)) {
00163 col = bad_color;
00164 ROS_DEBUG_STREAM("Setting bad color");
00165 } else {
00166 send = false;
00167 col = good_color;
00168 ROS_DEBUG_STREAM("Setting good color");
00169 }
00170
00171 visualization_msgs::MarkerArray arr;
00172 if(group.empty()) {
00173 collision_models_->getRobotMarkersGivenState(state,
00174 arr,
00175 col,
00176 "validator",
00177 ros::Duration(MARKER_DUR));
00178
00179 } else {
00180 const planning_models::KinematicModel::JointModelGroup* joint_model_group = collision_models_->getKinematicModel()->getModelGroup(group);
00181 if(joint_model_group == NULL) {
00182 ROS_INFO_STREAM("No joint group " << group);
00183 }
00184 std::vector<std::string> group_links = joint_model_group->getGroupLinkNames();
00185 collision_models_->getRobotMarkersGivenState(state,
00186 arr,
00187 col,
00188 group,
00189 ros::Duration(MARKER_DUR),
00190 &group_links);
00191 }
00192 if(send) {
00193 vis_marker_array_publisher_.publish(arr);
00194 }
00195 if(!group.empty()) {
00196 collision_models_->revertAllowedCollisionToDefault();
00197 }
00198 collision_models_->bodiesUnlock();
00199 }
00200
00201 void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state) {
00202 if((ros::Time::now()-last_update_time_).toSec() < (MARKER_DUR.toSec()/2.0)) {
00203 return;
00204 }
00205 last_update_time_ = ros::Time::now();
00206 if(!send_markers_) return;
00207 collision_models_->bodiesLock();
00208 if(group_name_1_.empty() && group_name_2_.empty()) {
00209 sendMarkersForGroup("");
00210 }
00211 if(!group_name_1_.empty()) {
00212 sendMarkersForGroup(group_name_1_);
00213 }
00214 if(!group_name_2_.empty()) {
00215 sendMarkersForGroup(group_name_2_);
00216 }
00217 collision_models_->bodiesUnlock();
00218 }
00219 protected:
00220
00221 ros::ServiceServer get_state_validity_service_;
00222
00223 planning_environment::CollisionModels* collision_models_;
00224 planning_environment::CollisionSpaceMonitor* csm_;
00225
00226 std::string group_name_1_;
00227 std::string group_name_2_;
00228
00229 bool send_markers_;
00230 ros::Time last_update_time_;
00231
00232 ros::NodeHandle root_handle_;
00233 ros::NodeHandle priv_handle_;
00234
00235 ros::Publisher vis_marker_publisher_;
00236 ros::Publisher vis_marker_array_publisher_;
00237
00238 tf::TransformListener tf_;
00239 };
00240
00241 int main(int argc, char **argv)
00242 {
00243 ros::init(argc, argv, "current_state_validator");
00244
00245 ros::AsyncSpinner spinner(1);
00246 spinner.start();
00247
00248 CurrentStateValidator cko;
00249 ros::waitForShutdown();
00250
00251 return 0;
00252 }
00253
