Skip to content

Commit

Permalink
Merge pull request moveit#46 from AcutronicRobotics/plan_execution
Browse files Browse the repository at this point in the history 
Port plan_execution
  • Loading branch information
@ahcorde
ahcorde authored Apr 17, 2019
2 parents 46ce183 + 8e98dcc commit 2b106bc
Show file tree
Hide file tree
Showing 7 changed files with 108 additions and 93 deletions.
8 changes: 4 additions & 4 deletions moveit_ros/planning/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,9 +47,9 @@ set(THIS_PACKAGE_INCLUDE_DIRS
set(libraries
moveit_rdf_loader
moveit_kinematics_plugin_loader
moveit_robot_model_loader
moveit_constraint_sampler_manager_loader
moveit_planning_pipeline
# moveit_robot_model_loader
# moveit_constraint_sampler_manager_loader
# moveit_planning_pipeline
moveit_trajectory_execution_manager
moveit_plan_execution
moveit_planning_scene_monitor
Expand Down Expand Up @@ -85,7 +85,7 @@ add_subdirectory(planning_pipeline)
add_subdirectory(planning_scene_monitor)
# add_subdirectory(planning_components_tools)
add_subdirectory(trajectory_execution_manager)
# add_subdirectory(plan_execution)
add_subdirectory(plan_execution)

install(
FILES
Expand Down
13 changes: 9 additions & 4 deletions moveit_ros/planning/plan_execution/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,17 @@ add_library(${MOVEIT_LIB_NAME}
src/plan_with_sensing.cpp
src/plan_execution.cpp)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
target_link_libraries(${MOVEIT_LIB_NAME}
ament_target_dependencies(${MOVEIT_LIB_NAME}
moveit_planning_pipeline
moveit_trajectory_execution_manager
moveit_planning_scene_monitor
${catkin_LIBRARIES} ${Boost_LIBRARIES}
rclcpp
Boost
)

install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION})
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)

install(DIRECTORY include/ DESTINATION include)
5 changes: 3 additions & 2 deletions moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,8 @@ class PlanExecution
};

PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution);
const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution,
const std::shared_ptr<rclcpp::Node> node);
~PlanExecution();

const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const
Expand Down Expand Up @@ -149,7 +150,7 @@ class PlanExecution
void doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus& status);
void successfulTrajectorySegmentExecution(const ExecutableMotionPlan* plan, std::size_t index);

ros::NodeHandle node_handle_;
std::shared_ptr<rclcpp::Node> node_;
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_;
Expand Down
2 changes: 1 addition & 1 deletion moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_representation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@

#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit_msgs/msg/move_it_error_codes.hpp>
#include <boost/function.hpp>

namespace plan_execution
Expand Down
7 changes: 4 additions & 3 deletions moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_with_sensing.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,8 @@ MOVEIT_CLASS_FORWARD(PlanWithSensing);
class PlanWithSensing
{
public:
PlanWithSensing(const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution);
PlanWithSensing(const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution,
const std::shared_ptr<rclcpp::Node> node);
~PlanWithSensing();

const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const
Expand Down Expand Up @@ -115,7 +116,7 @@ class PlanWithSensing
private:
bool lookAt(const std::set<collision_detection::CostSource>& cost_sources, const std::string& frame_id);

ros::NodeHandle node_handle_;
std::shared_ptr<rclcpp::Node> node_;
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;

std::unique_ptr<pluginlib::ClassLoader<moveit_sensor_manager::MoveItSensorManager> > sensor_manager_loader_;
Expand All @@ -127,7 +128,7 @@ class PlanWithSensing
unsigned int max_cost_sources_;

bool display_cost_sources_;
ros::Publisher cost_sources_publisher_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr cost_sources_publisher_;

boost::function<void()> before_look_callback_;

Expand Down
80 changes: 42 additions & 38 deletions moveit_ros/planning/plan_execution/src/plan_execution.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,45 +40,45 @@
#include <moveit/collision_detection/collision_tools.h>
#include <boost/algorithm/string/join.hpp>

#include <dynamic_reconfigure/server.h>
#include <moveit_ros_planning/PlanExecutionDynamicReconfigureConfig.h>
// #include <dynamic_reconfigure/server.h>
// #include <moveit_ros_planning/PlanExecutionDynamicReconfigureConfig.h>

namespace plan_execution
{
using namespace moveit_ros_planning;

class PlanExecution::DynamicReconfigureImpl
{
public:
DynamicReconfigureImpl(PlanExecution* owner)
: owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/plan_execution"))
: owner_(owner)//, dynamic_reconfigure_server_(ros::NodeHandle("~/plan_execution"))
{
dynamic_reconfigure_server_.setCallback(
boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
// dynamic_reconfigure_server_.setCallback(
// boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
}

private:
void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig& config, uint32_t level)
{
owner_->setMaxReplanAttempts(config.max_replan_attempts);
owner_->setTrajectoryStateRecordingFrequency(config.record_trajectory_state_frequency);
}
// void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig& config, uint32_t level)
// {
// owner_->setMaxReplanAttempts(config.max_replan_attempts);
// owner_->setTrajectoryStateRecordingFrequency(config.record_trajectory_state_frequency);
// }

PlanExecution* owner_;
dynamic_reconfigure::Server<PlanExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
// dynamic_reconfigure::Server<PlanExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
};
} // namespace plan_execution

plan_execution::PlanExecution::PlanExecution(
const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution)
: node_handle_("~")
const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution,
const std::shared_ptr<rclcpp::Node> node)
: node_(node)
, planning_scene_monitor_(planning_scene_monitor)
, trajectory_execution_manager_(trajectory_execution)
{
if (!trajectory_execution_manager_)
trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager(
planning_scene_monitor_->getRobotModel(), planning_scene_monitor_->getStateMonitor()));
planning_scene_monitor_->getRobotModel(), planning_scene_monitor_->getStateMonitor(),node_));

default_max_replan_attempts_ = 5;

Expand Down Expand Up @@ -173,7 +173,7 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p
do
{
replan_attempts++;
ROS_INFO_NAMED("plan_execution", "Planning attempt %u of at most %u", replan_attempts, max_replan_attempts);
RCLCPP_INFO(node_->get_logger(), "Planning attempt %u of at most %u", replan_attempts, max_replan_attempts);

if (opt.before_plan_callback_)
opt.before_plan_callback_();
Expand All @@ -200,8 +200,9 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p
if (plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA &&
opt.replan_delay_ > 0.0)
{
ros::WallDuration d(opt.replan_delay_);
d.sleep();
auto d = std::chrono::duration<double>(opt.replan_delay_);
auto period = std::chrono::milliseconds(int(d.count()*1000));
rclcpp::sleep_for(period);
}
continue;
}
Expand Down Expand Up @@ -236,29 +237,32 @@ void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& p
// othewrise, we wait (if needed)
if (opt.replan_delay_ > 0.0)
{
ROS_INFO_NAMED("plan_execution", "Waiting for a %lf seconds before attempting a new plan ...",
RCLCPP_INFO(node_->get_logger(), "Waiting for a %lf seconds before attempting a new plan ...",
opt.replan_delay_);
ros::WallDuration d(opt.replan_delay_);
d.sleep();
ROS_INFO_NAMED("plan_execution", "Done waiting");
auto d = std::chrono::duration<double>(opt.replan_delay_);
auto period = std::chrono::milliseconds(int(d.count()*1000));
rclcpp::sleep_for(period);
RCLCPP_INFO(node_->get_logger(), "Done waiting");
}
}
} while (!preempt_requested_ && max_replan_attempts > replan_attempts);

if (preempt_requested_)
{
ROS_DEBUG_NAMED("plan_execution", "PlanExecution was preempted");
RCLCPP_DEBUG(node_->get_logger(), "PlanExecution was preempted");
plan.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED;
}

if (opt.done_callback_)
opt.done_callback_();

if (plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
ROS_DEBUG_NAMED("plan_execution", "PlanExecution finished successfully.");
else
ROS_DEBUG_NAMED("plan_execution", "PlanExecution terminating with error code %d - '%s'", plan.error_code_.val,
if (plan.error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS){
RCLCPP_DEBUG(node_->get_logger(), "PlanExecution finished successfully.");
}
else{
RCLCPP_DEBUG(node_->get_logger(), "PlanExecution terminating with error code %d - '%s'", plan.error_code_.val,
getErrorCodeString(plan.error_code_).c_str());
}
}

bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan& plan)
Expand Down Expand Up @@ -296,7 +300,7 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false))
{
// Dave's debacle
ROS_INFO_NAMED("plan_execution", "Trajectory component '%s' is invalid",
RCLCPP_INFO(node_->get_logger(), "Trajectory component '%s' is invalid",
plan.plan_components_[path_segment.first].description_.c_str());

// call the same functions again, in verbose mode, to show what issues have been detected
Expand Down Expand Up @@ -328,7 +332,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni

if (!trajectory_execution_manager_)
{
ROS_ERROR_NAMED("plan_execution", "No trajectory execution manager");
RCLCPP_ERROR(node_->get_logger(), "No trajectory execution manager");
result.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED;
return result;
}
Expand Down Expand Up @@ -383,7 +387,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
if (!trajectory_execution_manager_->push(msg))
{
trajectory_execution_manager_->clear();
ROS_ERROR_STREAM_NAMED("plan_execution", "Apparently trajectory initialization failed");
RCLCPP_ERROR(node_->get_logger(), "Apparently trajectory initialization failed");
execution_complete_ = true;
result.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED;
return result;
Expand All @@ -403,9 +407,9 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1),
boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, _1));
// wait for path to be done, while checking that the path does not become invalid
ros::Rate r(100);
rclcpp::WallRate r(100);
path_became_invalid_ = false;
while (node_handle_.ok() && !execution_complete_ && !preempt_requested_ && !path_became_invalid_)
while (rclcpp::ok() && !execution_complete_ && !preempt_requested_ && !path_became_invalid_)
{
r.sleep();
// check the path if there was an environment update in the meantime
Expand All @@ -423,18 +427,18 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
// stop execution if needed
if (preempt_requested_)
{
ROS_INFO_NAMED("plan_execution", "Stopping execution due to preempt request");
RCLCPP_INFO(node_->get_logger(), "Stopping execution due to preempt request");
trajectory_execution_manager_->stopExecution();
}
else if (path_became_invalid_)
{
ROS_INFO_NAMED("plan_execution", "Stopping execution because the path to execute became invalid"
RCLCPP_INFO(node_->get_logger(), "Stopping execution because the path to execute became invalid"
"(probably the environment changed)");
trajectory_execution_manager_->stopExecution();
}
else if (!execution_complete_)
{
ROS_WARN_NAMED("plan_execution", "Stopping execution due to unknown reason."
RCLCPP_WARN(node_->get_logger(), "Stopping execution due to unknown reason."
"Possibly the node is about to shut down.");
trajectory_execution_manager_->stopExecution();
}
Expand Down Expand Up @@ -486,17 +490,17 @@ void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const E
{
if (plan->plan_components_.empty())
{
ROS_WARN_NAMED("plan_execution", "Length of provided motion plan is zero.");
RCLCPP_WARN(node_->get_logger(), "Length of provided motion plan is zero.");
return;
}

// if any side-effects are associated to the trajectory part that just completed, execute them
ROS_DEBUG_NAMED("plan_execution", "Completed '%s'", plan->plan_components_[index].description_.c_str());
RCLCPP_DEBUG(node_->get_logger(), "Completed '%s'", plan->plan_components_[index].description_.c_str());
if (plan->plan_components_[index].effect_on_success_)
if (!plan->plan_components_[index].effect_on_success_(plan))
{
// execution of side-effect failed
ROS_ERROR_NAMED("plan_execution", "Execution of path-completion side-effect failed. Preempting.");
RCLCPP_ERROR(node_->get_logger(), "Execution of path-completion side-effect failed. Preempting.");
preempt_requested_ = true;
return;
}
Expand Down
Loading

0 comments on commit 2b106bc

Please sign in to comment.