diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp index e74015c0bc..fe774dc01f 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp @@ -60,10 +60,12 @@ bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node, { stop_before_collision_ = node->declare_parameter("stop_before_collision", false); } - planning_scene_monitor_ = planning_scene_monitor; node_ = node; path_invalidation_event_send_ = false; num_iterations_stuck_ = 0; + + planning_scene_monitor_ = planning_scene_monitor; + return true; } @@ -106,6 +108,7 @@ ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajecto bool is_path_valid = false; // Lock the planning scene as briefly as possible { + planning_scene_monitor_->updateSceneWithCurrentState(); planning_scene_monitor::LockedPlanningSceneRO locked_planning_scene(planning_scene_monitor_); current_state = std::make_shared(locked_planning_scene->getCurrentState()); is_path_valid = locked_planning_scene->isPathValid(local_trajectory, local_trajectory.getGroupName(), false); diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp index 2c34cec5c4..0edc36e341 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -95,7 +95,9 @@ bool LocalPlannerComponent::initialize() // Start state and scene monitors planning_scene_monitor_->startSceneMonitor(config_.monitored_planning_scene_topic); planning_scene_monitor_->startWorldGeometryMonitor(config_.collision_object_topic); - planning_scene_monitor_->startStateMonitor(config_.joint_states_topic); + planning_scene_monitor_->startStateMonitor(config_.joint_states_topic, "/attached_collision_object"); + planning_scene_monitor_->monitorDiffs(true); + planning_scene_monitor_->stopPublishingPlanningScene(); // Load trajectory operator plugin try @@ -264,8 +266,6 @@ void LocalPlannerComponent::executeIteration() // If the planner received an action request and a global solution it starts to plan locally case LocalPlannerState::LOCAL_PLANNING_ACTIVE: { - planning_scene_monitor_->updateSceneWithCurrentState(); - // Read current robot state const moveit::core::RobotState current_robot_state = [this] { planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor_);