40 #include <boost/algorithm/string/join.hpp> 42 #include <dynamic_reconfigure/server.h> 43 #include <moveit_ros_planning/SenseForPlanDynamicReconfigureConfig.h> 53 : owner_(owner), dynamic_reconfigure_server_(
ros::NodeHandle(
"~/sense_for_plan"))
55 dynamic_reconfigure_server_.setCallback(
56 boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback,
this, _1, _2));
62 owner_->setMaxSafePathCost(config.max_safe_path_cost);
63 owner_->setMaxCostSources(config.max_cost_sources);
64 owner_->setMaxLookAttempts(config.max_look_attempts);
65 owner_->setDiscardOverlappingCostSources(config.discard_overlapping_cost_sources);
74 const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution)
75 : node_handle_(
"~"), trajectory_execution_manager_(trajectory_execution)
92 "moveit_core",
"moveit_sensor_manager::MoveItSensorManager"));
96 ROS_ERROR_STREAM(
"Exception while creating sensor manager plugin loader: " << ex.what());
108 ROS_ERROR_STREAM(
"Exception while loading sensor manager '" << manager <<
"': " << ex.what());
113 std::vector<std::string> sensors;
115 ROS_INFO_STREAM(
"PlanWithSensing is aware of the following sensors: " << boost::algorithm::join(sensors,
", "));
141 unsigned int max_look_attempts,
double max_safe_path_cost)
143 if (max_safe_path_cost <= std::numeric_limits<double>::epsilon())
146 if (max_look_attempts == 0)
149 double previous_cost = 0.0;
150 unsigned int look_attempts = 0;
154 bool just_looked_around =
false;
159 bool look_around_failed =
false;
165 bool solved = motion_planner(plan);
166 if (!solved || plan.
error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
170 std::set<collision_detection::CostSource> cost_sources;
178 std::set<collision_detection::CostSource> cost_sources_i;
182 cost_sources.insert(cost_sources_i.begin(), cost_sources_i.end());
185 std::set<collision_detection::CostSource> other;
186 other.swap(cost_sources);
188 for (std::set<collision_detection::CostSource>::iterator it = other.begin(); j <
max_cost_sources_; ++it, ++j)
189 cost_sources.insert(*it);
197 visualization_msgs::MarkerArray arr;
203 ROS_DEBUG(
"The total cost of the trajectory is %lf.", cost);
204 if (previous_cost > 0.0)
205 ROS_DEBUG(
"The change in the trajectory cost is %lf after the perception step.", cost - previous_cost);
206 if (cost > max_safe_path_cost && look_attempts < max_look_attempts)
209 ROS_INFO(
"The cost of the trajectory is %lf, which is above the maximum safe cost of %lf. Attempt %u (of at most " 210 "%u) at looking around.",
211 cost, max_safe_path_cost, look_attempts, max_look_attempts);
214 if (looked_at_result)
215 ROS_INFO(
"Sensor was succesfully actuated. Attempting to recompute a motion plan.");
218 if (look_around_failed)
219 ROS_WARN(
"Looking around seems to keep failing. Giving up.");
221 ROS_WARN(
"Looking around seems to have failed. Attempting to recompute a motion plan anyway.");
223 if (looked_at_result || !look_around_failed)
225 previous_cost = cost;
226 just_looked_around =
true;
228 look_around_failed = !looked_at_result;
230 if (just_looked_around)
234 if (cost > max_safe_path_cost)
236 plan.
error_code_.val = moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA;
247 const std::string& frame_id)
251 ROS_WARN(
"It seems looking around would be useful, but no MoveIt! Sensor Manager is loaded. Did you set " 252 "~moveit_sensor_manager ?");
259 std::vector<std::string> names;
261 geometry_msgs::PointStamped point;
262 for (std::size_t i = 0; i < names.size(); ++i)
266 point.header.frame_id = frame_id;
268 moveit_msgs::RobotTrajectory sensor_trajectory;
269 if (
sensor_manager_->pointSensorTo(names[i], point, sensor_trajectory))
unsigned int max_cost_sources_
void getCostMarkers(visualization_msgs::MarkerArray &arr, const std::string &frame_id, std::set< CostSource > &cost_sources)
bool lookAt(const std::set< collision_detection::CostSource > &cost_sources, const std::string &frame_id)
void publish(const boost::shared_ptr< M > &message) const
double discard_overlapping_cost_sources_
double getTotalCost(const std::set< CostSource > &cost_sources)
moveit_msgs::MoveItErrorCodes error_code_
An error code reflecting what went wrong (if anything)
std::unique_ptr< pluginlib::ClassLoader< moveit_sensor_manager::MoveItSensorManager > > sensor_manager_loader_
boost::function< bool(ExecutableMotionPlan &plan)> ExecutableMotionPlanComputationFn
The signature of a function that can compute a motion plan.
ros::Publisher cost_sources_publisher_
unsigned int default_max_look_attempts_
planning_scene::PlanningSceneConstPtr planning_scene_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
moveit_sensor_manager::MoveItSensorManagerPtr sensor_manager_
bool computePlan(ExecutableMotionPlan &plan, const ExecutableMotionPlanComputationFn &motion_planner, unsigned int max_look_attempts, double max_safe_path_cost)
bool getSensorPositioning(geometry_msgs::Point &point, const std::set< CostSource > &cost_sources)
void displayCostSources(bool flag)
boost::function< void()> before_look_callback_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
double default_max_safe_path_cost_
bool display_cost_sources_
#define ROS_DEBUG_STREAM(args)
bool isTrajectoryEmpty(const moveit_msgs::RobotTrajectory &trajectory)
#define ROS_INFO_STREAM(args)
ros::NodeHandle node_handle_
This namespace includes functionality specific to the execution and monitoring of motion plans...
bool getParam(const std::string &key, std::string &s) const
A generic representation on what a computed motion plan looks like.
PlanWithSensing(const trajectory_execution_manager::TrajectoryExecutionManagerPtr &trajectory_execution)
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
std::vector< ExecutableTrajectory > plan_components_
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
DynamicReconfigureImpl * reconfigure_impl_
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_