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
00035
00036
00037
00038 #include "planning_environment/monitors/environment_server_setup.h"
00039 #include "planning_environment/monitors/planning_monitor.h"
00040
00041 #include <trajectory_msgs/JointTrajectory.h>
00042 #include <sensor_msgs/JointState.h>
00043 #include <motion_planning_msgs/DisplayTrajectory.h>
00044
00045 #include <planning_environment_msgs/GetRobotState.h>
00046 #include <planning_environment_msgs/GetJointTrajectoryValidity.h>
00047 #include <planning_environment_msgs/GetStateValidity.h>
00048 #include <planning_environment_msgs/GetJointsInGroup.h>
00049 #include <planning_environment_msgs/GetGroupInfo.h>
00050 #include <planning_environment_msgs/GetEnvironmentSafety.h>
00051 #include <planning_environment_msgs/ContactInformation.h>
00052 #include "planning_environment/monitors/environment_server_setup.h"
00053 #include <valarray>
00054 #include <visualization_msgs/Marker.h>
00055
00056 #include <visualization_msgs/MarkerArray.h>
00057
00058 namespace planning_environment
00059 {
00060 class EnvironmentServer
00061 {
00062 public:
00063 EnvironmentServer(EnvironmentServerSetup &setup) : private_handle_("~"),setup_(setup)
00064 {
00065 int velocity_history_size;
00066 private_handle_.param<int>("velocity_history_size", velocity_history_size, 20);
00067
00068 planning_monitor_ = setup_.planning_monitor_;
00069 planning_monitor_->getEnvironmentModel()->setVerbose(false);
00070
00071 planning_monitor_->waitForState();
00072 planning_monitor_->startEnvironmentMonitor();
00073 while(!planning_monitor_->haveMap() && setup_.use_collision_map_){
00074 ros::Duration().fromSec(0.05).sleep();
00075 }
00076
00077 tf_ = &setup_.tf_;
00078
00079 get_trajectory_validity_service_ = private_handle_.advertiseService("get_trajectory_validity", &EnvironmentServer::getTrajectoryValidity, this);
00080 get_env_safety_service_ = private_handle_.advertiseService("get_environment_safety", &EnvironmentServer::getEnvironmentSafety, this);
00081 get_execution_safety_service_ = private_handle_.advertiseService("get_execution_safety", &EnvironmentServer::getExecutionSafety, this);
00082 get_joints_in_group_service_ = private_handle_.advertiseService("get_joints_in_group", &EnvironmentServer::getJointsInGroup, this);
00083
00084 get_group_info_service_ = private_handle_.advertiseService("get_group_info", &EnvironmentServer::getGroupInfo, this);
00085
00086 get_robot_state_service_ = private_handle_.advertiseService("get_robot_state", &EnvironmentServer::getRobotState, this);
00087 get_state_validity_service_ = private_handle_.advertiseService("get_state_validity", &EnvironmentServer::getStateValidity, this);
00088 allowed_contact_regions_publisher_ = private_handle_.advertise<visualization_msgs::MarkerArray>("allowed_contact_regions_array", 128);
00089
00090 velocity_history_.resize(velocity_history_size);
00091 velocity_history_index_ = 0;
00092
00093 joint_state_subscriber_ = root_handle_.subscribe("joint_states", 1, &planning_environment::EnvironmentServer::jointStateCallback, this);
00094
00095 vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("environment_server_contact_markers", 128);
00096
00097 planning_monitor_->setOnCollisionContactCallback(boost::bind(&EnvironmentServer::contactFound, this, _1));
00098
00099 ROS_INFO("Environment server started");
00100 }
00101
00102 virtual ~EnvironmentServer()
00103 {
00104 }
00105
00106 private:
00107
00111 void contactFound(collision_space::EnvironmentModel::Contact &contact)
00112 {
00113 static int count = 0;
00114
00115 std::string ns_name;
00116 planning_environment_msgs::ContactInformation contact_info;
00117 contact_info.header.frame_id = planning_monitor_->getWorldFrameId();
00118 if(contact.link1 != NULL) {
00119
00120 if(contact.link1_attached_body_index == 0) {
00121 ns_name += contact.link1->getName()+"+";
00122 contact_info.contact_body_1 = contact.link1->getName();
00123 contact_info.attached_body_1 = "";
00124 contact_info.body_type_1 = planning_environment_msgs::ContactInformation::ROBOT_LINK;
00125 } else {
00126 if(contact.link1->getAttachedBodyModels().size() < contact.link1_attached_body_index) {
00127 ROS_ERROR("Link doesn't have attached body with indicated index");
00128 } else {
00129 ns_name += contact.link1->getAttachedBodyModels()[contact.link1_attached_body_index-1]->getName()+"+";
00130 contact_info.contact_body_1 = contact.link1->getName();
00131 contact_info.attached_body_1 = contact.link1->getAttachedBodyModels()[contact.link1_attached_body_index-1]->getName();
00132 contact_info.body_type_1 = planning_environment_msgs::ContactInformation::ATTACHED_BODY;
00133 }
00134 }
00135 }
00136
00137 if(contact.link2 != NULL) {
00138
00139 if(contact.link2_attached_body_index == 0) {
00140 ns_name += contact.link2->getName();
00141 contact_info.contact_body_2 = contact.link2->getName();
00142 contact_info.attached_body_2 = "";
00143 contact_info.body_type_2 = planning_environment_msgs::ContactInformation::ROBOT_LINK;
00144 } else {
00145 if(contact.link2->getAttachedBodyModels().size() < contact.link2_attached_body_index) {
00146 ROS_ERROR("Link doesn't have attached body with indicated index");
00147 } else {
00148 ns_name += contact.link2->getAttachedBodyModels()[contact.link2_attached_body_index-1]->getName();
00149 contact_info.contact_body_2 = contact.link2->getName();
00150 contact_info.attached_body_2 = contact.link2->getAttachedBodyModels()[contact.link2_attached_body_index-1]->getName();
00151 contact_info.body_type_2 = planning_environment_msgs::ContactInformation::ATTACHED_BODY;
00152 }
00153 }
00154 }
00155
00156 if(!contact.object_name.empty()) {
00157
00158 ns_name += contact.object_name;
00159 contact_info.contact_body_2 = contact.object_name;
00160 contact_info.attached_body_2 = "";
00161 contact_info.body_type_2 = planning_environment_msgs::ContactInformation::OBJECT;
00162 }
00163
00164 visualization_msgs::Marker mk;
00165 mk.header.stamp = planning_monitor_->lastPoseUpdate();
00166 mk.header.frame_id = planning_monitor_->getWorldFrameId();
00167 mk.ns = ns_name;
00168 mk.id = count++;
00169 mk.type = visualization_msgs::Marker::SPHERE;
00170 mk.action = visualization_msgs::Marker::ADD;
00171 mk.pose.position.x = contact.pos.x();
00172 mk.pose.position.y = contact.pos.y();
00173 mk.pose.position.z = contact.pos.z();
00174 mk.pose.orientation.w = 1.0;
00175
00176 mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
00177
00178 mk.color.a = 0.6;
00179 mk.color.r = 1.0;
00180 mk.color.g = 0.8;
00181 mk.color.b = 0.04;
00182
00183
00184 contact_info.position.x = contact.pos.x();
00185 contact_info.position.y = contact.pos.y();
00186 contact_info.position.z = contact.pos.z();
00187 contact_info.depth = contact.depth;
00188
00189
00190
00191
00192
00193
00194
00195
00196 contact_information_.push_back(contact_info);
00197 vis_marker_publisher_.publish(mk);
00198 }
00199
00200 bool getEnvironmentSafety(planning_environment_msgs::GetEnvironmentSafety::Request &req,
00201 planning_environment_msgs::GetEnvironmentSafety::Response &res)
00202 {
00203 planning_monitor_->isEnvironmentSafe(res.error_code);
00204 return true;
00205 }
00206
00207 bool getRobotState(planning_environment_msgs::GetRobotState::Request &req,
00208 planning_environment_msgs::GetRobotState::Response &res)
00209 {
00210 planning_monitor_->getCurrentRobotState(res.robot_state);
00211 return true;
00212 }
00213
00214 bool getJointsInGroup(planning_environment_msgs::GetJointsInGroup::Request &req,
00215 planning_environment_msgs::GetJointsInGroup::Response &res)
00216 {
00217 ROS_DEBUG("Looking for joints in group %s",req.group_name.c_str());
00218 const planning_models::KinematicModel::JointModelGroup *jg = planning_monitor_->getKinematicModel()->getModelGroup(req.group_name);
00219 if(jg)
00220 {
00221 res.joint_names = jg->getJointModelNames();
00222 res.error_code.val = res.error_code.SUCCESS;
00223 return true;
00224 }
00225 res.error_code.val = res.error_code.INVALID_GROUP_NAME;
00226 return true;
00227 }
00228
00229 bool getGroupInfo(planning_environment_msgs::GetGroupInfo::Request &req,
00230 planning_environment_msgs::GetGroupInfo::Response &res)
00231 {
00232 if(req.group_name.empty()) {
00233 ROS_DEBUG_STREAM("Getting info for all groups");
00234 } else {
00235 ROS_DEBUG_STREAM("Getting info for group name " << req.group_name);
00236 }
00237 if(req.group_name.empty()) {
00238 res.joint_names = setup_.collision_models_->getGroupJointUnion();
00239 res.link_names = setup_.collision_models_->getGroupLinkUnion();
00240 res.error_code.val = res.error_code.SUCCESS;
00241 } else {
00242 const std::map< std::string, std::vector<std::string> > planning_group_joints = setup_.collision_models_->getPlanningGroupJoints();
00243 const std::map< std::string, std::vector<std::string> > planning_group_links = setup_.collision_models_->getPlanningGroupLinks();
00244 if(planning_group_joints.find(req.group_name) == planning_group_joints.end() ||
00245 planning_group_links.find(req.group_name) == planning_group_links.end()) {
00246 res.error_code.val = res.error_code.INVALID_GROUP_NAME;
00247 } else {
00248 res.joint_names = planning_group_joints.find(req.group_name)->second;
00249 res.link_names = planning_group_links.find(req.group_name)->second;
00250 res.error_code.val = res.error_code.SUCCESS;
00251 }
00252 }
00253 return true;
00254 }
00255
00256 bool getTrajectoryValidity(planning_environment_msgs::GetJointTrajectoryValidity::Request &req,
00257 planning_environment_msgs::GetJointTrajectoryValidity::Response &res)
00258 {
00259 contact_information_.clear();
00260 planning_monitor_->prepareForValidityChecks(req.trajectory.joint_names,
00261 req.ordered_collision_operations,
00262 req.allowed_contacts,
00263 req.path_constraints,
00264 req.goal_constraints,
00265 req.link_padding,
00266 res.error_code);
00267 if(res.error_code.val != res.error_code.SUCCESS)
00268 {
00269 ROS_ERROR("Could not prepare planning_environment");
00270 return true;;
00271 }
00272 int flag = getCheckFlag(req);
00273 if(planning_monitor_->isTrajectoryValid(req.trajectory,
00274 req.robot_state,
00275 (unsigned int) 0,
00276 (unsigned int) req.trajectory.points.size()-1,
00277 flag,
00278 true,
00279 res.error_code,
00280 res.trajectory_error_codes))
00281 {
00282 res.error_code.val = res.error_code.SUCCESS;
00283 ROS_DEBUG("Trajectory is valid");
00284 }
00285 res.contacts = contact_information_;
00286 planning_monitor_->revertToDefaultState();
00287 return true;
00288 }
00289
00290 bool getStateValidity(planning_environment_msgs::GetStateValidity::Request &req,
00291 planning_environment_msgs::GetStateValidity::Response &res)
00292 {
00293 contact_information_.clear();
00294 std::vector<std::string> names = req.robot_state.joint_state.name;
00295 if(!req.robot_state.multi_dof_joint_state.joint_names.empty()) {
00296 names.insert(names.end(),req.robot_state.multi_dof_joint_state.joint_names.begin(),
00297 req.robot_state.multi_dof_joint_state.joint_names.end());
00298 }
00299 if(names.empty()) {
00300 planning_monitor_->getKinematicModel()->getJointModelNames(names);
00301 }
00302 planning_monitor_->prepareForValidityChecks(names,
00303 req.ordered_collision_operations,
00304 req.allowed_contacts,
00305 req.path_constraints,
00306 req.goal_constraints,
00307 req.link_padding,
00308 res.error_code);
00309 if(res.error_code.val != res.error_code.SUCCESS)
00310 {
00311 ROS_ERROR("Could not prepare planning environment");
00312 return true;
00313 }
00314 int flag = getCheckFlag(req);
00315 if(planning_monitor_->isStateValid(req.robot_state,flag,true,res.error_code))
00316 {
00317 res.error_code.val = res.error_code.SUCCESS;
00318 }
00319 res.contacts = contact_information_;
00320
00321 planning_monitor_->revertToDefaultState();
00322 return true;
00323 }
00324
00325 bool getExecutionSafety(planning_environment_msgs::GetJointTrajectoryValidity::Request &req,
00326 planning_environment_msgs::GetJointTrajectoryValidity::Response &res)
00327 {
00328 ros::Time start = ros::Time::now();
00329 contact_information_.clear();
00330 planning_monitor_->prepareForValidityChecks(req.trajectory.joint_names,
00331 req.ordered_collision_operations,
00332 req.allowed_contacts,
00333 req.path_constraints,
00334 req.goal_constraints,
00335 req.link_padding,
00336 res.error_code);
00337 if(res.error_code.val != res.error_code.SUCCESS)
00338 {
00339 ROS_ERROR("Could not prepare planning monitor to check validity");
00340 return true;
00341 }
00342 if(!planning_monitor_->isEnvironmentSafe(res.error_code))
00343 return true;
00344
00345 int current_position_index(0);
00346
00347 current_position_index = planning_monitor_->closestStateOnTrajectory(req.trajectory,
00348 req.robot_state,
00349 current_position_index,
00350 req.trajectory.points.size() - 1,
00351 res.error_code);
00352 if (current_position_index < 0)
00353 {
00354 ROS_WARN("Unable to identify current state in path");
00355 current_position_index = 0;
00356 }
00357
00358 int flag = getCheckFlag(req);
00359 bool valid = false;
00360 valid = planning_monitor_->isTrajectoryValid(req.trajectory,
00361 req.robot_state,
00362 current_position_index,
00363 req.trajectory.points.size() - 1,
00364 flag,
00365 false,
00366 res.error_code,res.trajectory_error_codes);
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377 res.contacts = contact_information_;
00378 planning_monitor_->revertToDefaultState();
00379 return true;
00380 }
00381
00382 int getCheckFlag(const planning_environment_msgs::GetJointTrajectoryValidity::Request &req)
00383 {
00384 int result(0);
00385 if(req.check_collisions)
00386 result = result | planning_environment::PlanningMonitor::COLLISION_TEST;
00387 if(req.check_path_constraints)
00388 result = result | planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST;
00389 if(req.check_goal_constraints)
00390 result = result | planning_environment::PlanningMonitor::GOAL_CONSTRAINTS_TEST;
00391 if(req.check_joint_limits)
00392 result = result | planning_environment::PlanningMonitor::JOINT_LIMITS_TEST;
00393 if(req.check_full_trajectory)
00394 result = result | planning_environment::PlanningMonitor::CHECK_FULL_TRAJECTORY;
00395 return result;
00396 }
00397
00398 int getCheckFlag(const planning_environment_msgs::GetStateValidity::Request &req)
00399 {
00400 int result(0);
00401 if(req.check_collisions)
00402 result = result | planning_environment::PlanningMonitor::COLLISION_TEST;
00403 if(req.check_path_constraints)
00404 result = result | planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST;
00405 if(req.check_goal_constraints)
00406 result = result | planning_environment::PlanningMonitor::GOAL_CONSTRAINTS_TEST;
00407 if(req.check_joint_limits)
00408 result = result | planning_environment::PlanningMonitor::JOINT_LIMITS_TEST;
00409 return result;
00410 }
00411
00412 void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
00413 {
00414 velocity_history_[velocity_history_index_ % velocity_history_.size()] = planning_monitor_->getTotalVelocity();
00415 velocity_history_index_++;
00416 }
00417
00418 void visualizeAllowedContactRegions(const std::vector<motion_planning_msgs::AllowedContactSpecification> &allowed_contacts)
00419 {
00420 static int count = 0;
00421 visualization_msgs::MarkerArray mk;
00422 mk.markers.resize(allowed_contacts.size());
00423 for(unsigned int i=0; i < allowed_contacts.size(); i++)
00424 {
00425 bool valid_shape = true;
00426 mk.markers[i].header.stamp = ros::Time::now();
00427 mk.markers[i].header.frame_id = allowed_contacts[i].pose_stamped.header.frame_id;
00428 mk.markers[i].ns = allowed_contacts[i].name;
00429 mk.markers[i].id = count++;
00430 if(allowed_contacts[i].shape.type == geometric_shapes_msgs::Shape::SPHERE)
00431 {
00432 mk.markers[i].type = visualization_msgs::Marker::SPHERE;
00433 if(allowed_contacts[i].shape.dimensions.size() >= 1)
00434 mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[0];
00435 else
00436 valid_shape = false;
00437 }
00438 else if (allowed_contacts[i].shape.type == geometric_shapes_msgs::Shape::BOX)
00439 {
00440 mk.markers[i].type = visualization_msgs::Marker::CUBE;
00441 if(allowed_contacts[i].shape.dimensions.size() >= 3)
00442 {
00443 mk.markers[i].scale.x = allowed_contacts[i].shape.dimensions[0];
00444 mk.markers[i].scale.y = allowed_contacts[i].shape.dimensions[1];
00445 mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[2];
00446 }
00447 else
00448 valid_shape = false;
00449 }
00450 else if (allowed_contacts[i].shape.type == geometric_shapes_msgs::Shape::CYLINDER)
00451 {
00452 mk.markers[i].type = visualization_msgs::Marker::CYLINDER;
00453 if(allowed_contacts[i].shape.dimensions.size() >= 2)
00454 {
00455 mk.markers[i].scale.x = allowed_contacts[i].shape.dimensions[0];
00456 mk.markers[i].scale.y = allowed_contacts[i].shape.dimensions[0];
00457 mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[1];
00458 }
00459 else
00460 valid_shape = false;
00461 }
00462 else
00463 {
00464 mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = 0.01;
00465 valid_shape = false;
00466 }
00467
00468 mk.markers[i].action = visualization_msgs::Marker::ADD;
00469 mk.markers[i].pose = allowed_contacts[i].pose_stamped.pose;
00470 if(!valid_shape)
00471 {
00472 mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = 0.01;
00473 mk.markers[i].color.a = 0.5;
00474 mk.markers[i].color.r = 1.0;
00475 mk.markers[i].color.g = 0.04;
00476 mk.markers[i].color.b = 0.04;
00477 }
00478 else
00479 {
00480 mk.markers[i].color.a = 0.5;
00481 mk.markers[i].color.r = 0.04;
00482 mk.markers[i].color.g = 1.0;
00483 mk.markers[i].color.b = 0.04;
00484 }
00485
00486 }
00487 allowed_contact_regions_publisher_.publish(mk);
00488 }
00489
00490 private:
00491
00492 ros::NodeHandle root_handle_, private_handle_;
00493 EnvironmentServerSetup &setup_;
00494 planning_environment::PlanningMonitor *planning_monitor_;
00495 tf::TransformListener *tf_;
00496
00497 bool show_collisions_;
00498 bool allow_valid_collisions_;
00499 std::vector<collision_space::EnvironmentModel::AllowedContact> allowed_contacts_;
00500
00501 ros::Publisher vis_marker_publisher_;
00502 ros::ServiceServer get_trajectory_validity_service_;
00503 ros::ServiceServer get_env_safety_service_;
00504 ros::ServiceServer get_execution_safety_service_;
00505 ros::ServiceServer set_constraints_service_;
00506 ros::ServiceServer get_joints_in_group_service_;
00507 ros::ServiceServer get_group_info_service_;
00508 ros::ServiceServer get_robot_state_service_;
00509 ros::ServiceServer get_state_validity_service_;
00510 ros::Publisher allowed_contact_regions_publisher_;
00511
00512 ros::Subscriber joint_state_subscriber_;
00513
00514 std::valarray<double> velocity_history_;
00515 unsigned int velocity_history_index_;
00516
00517 std::vector<planning_environment_msgs::ContactInformation> contact_information_
00518 ;
00519 };
00520 }
00521
00522 int main(int argc, char** argv)
00523 {
00524 ros::init(argc, argv, "environment_monitor");
00525 planning_environment::EnvironmentServerSetup setup;
00526 ros::AsyncSpinner spinner(4);
00527 spinner.start();
00528 if (!setup.configure())
00529 {
00530 ros::shutdown();
00531 return 0;
00532 }
00533 planning_environment::EnvironmentServer environment_monitor(setup);
00534 ros::waitForShutdown();
00535 return 0;
00536 }
