Skip to content

[hybrid planning] improve planning scene monitoring #1090

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
sjahr merged 12 commits into from
Apr 4, 2023
Merged

[hybrid planning] improve planning scene monitoring #1090

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
caf7389
Create new PSM in local constraint solver. Different type of collisio…
Feb 23, 2022
8047704
Small boolean logic fixup
Feb 24, 2022
4d5f68d
Don't configure planning scene monitor twice and pass scene as const
sjahr Mar 13, 2023
00f98d6
Remove not required call of updateSceneWithCurrentState()
sjahr Mar 15, 2023
009fad3
Revert PlanningSceneMonitorConstPtr to PlanningSceneMonitorPtr
sjahr Mar 21, 2023
68a854e
Set planning_scene_monitor update rate
sjahr Mar 21, 2023
c7ad8ca
Decrease planning scene update rate
sjahr Mar 24, 2023
69f844c
Use `updateSceneWithCurrentState()` in psm
AndyZe Mar 25, 2023
45ab01e
Merge branch 'main' into andyz/hp_psm
AndyZe Mar 25, 2023
2574cfa
Merge branch 'main' into andyz/hp_psm
sjahr Mar 28, 2023
dbe137d
Merge branch 'main' into andyz/hp_psm
AndyZe Apr 3, 2023
1055da2
Revert the manner of collision checking
AndyZe Apr 3, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion .../hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,10 +60,12 @@ bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node,
{
stop_before_collision_ = node->declare_parameter<bool>("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;
}

Expand Down Expand Up @@ -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<moveit::core::RobotState>(locked_planning_scene->getCurrentState());
is_path_valid = locked_planning_scene->isPathValid(local_trajectory, local_trajectory.getGroupName(), false);
Expand Down
6 changes: 3 additions & 3 deletions ...ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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_);
Expand Down