Skip to content

Commit 7393752

Browse files
rhaschkerhaschke
committed
Merge branch master into ros2
2 parents 5dd170b + 55c4b52 commit 7393752

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

54 files changed

+906
-342
lines changed

‎.github/workflows/ci.yaml‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,7 @@ jobs:
5353
name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
5454
runs-on: ubuntu-latest
5555
steps:
56-
- uses: actions/checkout@v3
56+
- uses: actions/checkout@v4
5757
with:
5858
submodules: recursive
5959

@@ -70,7 +70,7 @@ jobs:
7070

7171
- id: ici
7272
name: Run industrial_ci
73-
uses: rhaschke/industrial_ci@clang-tidy
73+
uses: rhaschke/industrial_ci@master
7474
env: ${{ matrix.env }}
7575

7676
- name: Upload ici's target_ws/install folder
@@ -80,14 +80,14 @@ jobs:
8080
subdir: target_ws/install
8181

8282
- name: Upload test artifacts (on failure)
83-
uses: actions/upload-artifact@v3
83+
uses: actions/upload-artifact@v4
8484
if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
8585
with:
8686
name: test-results-${{ matrix.env.IMAGE }}${{ matrix.env.NAME && '-' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && '-clang-tidy' || '' }}
8787
path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
8888

8989
- name: Upload clang-tidy fixes (on failure)
90-
uses: actions/upload-artifact@v3
90+
uses: actions/upload-artifact@v4
9191
if: failure() && steps.ici.outputs.clang_tidy_checks
9292
with:
9393
name: clang-tidy-fixes.yaml

‎.github/workflows/format.yaml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,12 +13,12 @@ jobs:
1313
name: pre-commit
1414
runs-on: ubuntu-latest
1515
steps:
16-
- uses: actions/checkout@v3
16+
- uses: actions/checkout@v4
1717
with:
1818
submodules: recursive
1919
- name: Install clang-format-12
2020
run: sudo apt-get install clang-format-12
21-
- uses: pre-commit/[email protected].0
21+
- uses: pre-commit/[email protected].1
2222
id: precommit
2323
- name: Upload pre-commit changes
2424
if: failure() && steps.precommit.outcome == 'failure'

‎.github/workflows/prerelease.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ jobs:
3434
# free up a lot of stuff from /usr/local
3535
sudo rm -rf /usr/local
3636
df -h
37-
- uses: actions/checkout@v3
37+
- uses: actions/checkout@v4
3838
with:
3939
submodules: recursive
4040
- name: industrial_ci

‎README.md‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,14 @@ It draws on the planning capabilities of [MoveIt](https://moveit.ros.org/) to so
55
A common interface, based on MoveIt's PlanningScene is used to pass solution hypotheses between stages.
66
The framework enables the hierarchical organization of basic stages using *containers*, allowing for sequential as well as parallel compositions.
77

8+
## Branches
9+
10+
This repository provides the following branches:
11+
12+
- **master**: ROS 1 development
13+
- **ros2**: ROS 2 development, compatible with MoveIt 2 `main`
14+
- **humble**: ROS 2 stable branch for Humble support
15+
816
## Videos
917

1018
- Demo video associated with [ICRA 2019 paper](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf)

‎capabilities/src/execute_task_solution_capability.cpp‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -175,11 +175,15 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
175175
exec_traj.trajectory->setRobotTrajectoryMsg(state, sub_traj.trajectory);
176176

177177
/* TODO add action feedback and markers */
178-
exec_traj.effect_on_success = [this, sub_traj,
178+
exec_traj.effect_on_success = [this,
179+
&scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff),
179180
description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
180-
if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
181+
scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
182+
scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
183+
184+
if (!moveit::core::isEmpty(scene_diff)) {
181185
RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
182-
return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
186+
return context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff);
183187
}
184188
return true;
185189
};

‎core/include/moveit/task_constructor/solvers/cartesian_path.h‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -63,14 +63,14 @@ class CartesianPath : public PlannerInterface
6363

6464
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
6565

66-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
67-
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
66+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
67+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
6969

70-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
71-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
72-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
73-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
70+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
71+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
72+
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
73+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
7474

7575
std::string getPlannerId() const override { return "CartesianPath"; }
7676
};

‎core/include/moveit/task_constructor/solvers/joint_interpolation.h‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -58,14 +58,14 @@ class JointInterpolationPlanner : public PlannerInterface
5858

5959
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
6060

61-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
62-
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
63-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
61+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
62+
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
63+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
6464

65-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
66-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
67-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
65+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
66+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
67+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
6969

7070
std::string getPlannerId() const override { return "JointInterpolationPlanner"; }
7171
};

‎core/include/moveit/task_constructor/solvers/multi_planner.h‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -63,14 +63,14 @@ class MultiPlanner : public PlannerInterface, public std::vector<solvers::Planne
6363

6464
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
6565

66-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
67-
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
66+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
67+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
6969

70-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
71-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
72-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
73-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
70+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
71+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
72+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
73+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
7474
};
7575
} // namespace solvers
7676
} // namespace task_constructor

‎core/include/moveit/task_constructor/solvers/pipeline_planner.h‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -115,10 +115,10 @@ class PipelinePlanner : public PlannerInterface
115115
* \param [in] path_constraints Path contraints for the planning problem
116116
* \return true If the solver found a successful solution for the given planning problem
117117
*/
118-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
119-
const core::JointModelGroup* joint_model_group, double timeout,
120-
robot_trajectory::RobotTrajectoryPtr& result,
121-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
118+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
119+
const core::JointModelGroup* joint_model_group, double timeout,
120+
robot_trajectory::RobotTrajectoryPtr& result,
121+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
122122

123123
/** \brief Plan a trajectory from a planning scene 'from' to a Cartesian target pose with an offset
124124
* \param [in] from Start planning scene
@@ -131,11 +131,11 @@ class PipelinePlanner : public PlannerInterface
131131
* \param [in] path_constraints Path contraints for the planning problem
132132
* \return true If the solver found a successful solution for the given planning problem
133133
*/
134-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
135-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
136-
const moveit::core::JointModelGroup* joint_model_group, double timeout,
137-
robot_trajectory::RobotTrajectoryPtr& result,
138-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
134+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
135+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
136+
const moveit::core::JointModelGroup* joint_model_group, double timeout,
137+
robot_trajectory::RobotTrajectoryPtr& result,
138+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
139139

140140
std::string getPlannerId() const override { return last_successful_planner_; }
141141

@@ -149,11 +149,11 @@ class PipelinePlanner : public PlannerInterface
149149
* \param [in] path_constraints Path contraints for the planning problem
150150
* \return true if the solver found a successful solution for the given planning problem
151151
*/
152-
bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
153-
const moveit::core::JointModelGroup* joint_model_group,
154-
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
155-
robot_trajectory::RobotTrajectoryPtr& result,
156-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
152+
Result plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
153+
const moveit::core::JointModelGroup* joint_model_group,
154+
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
155+
robot_trajectory::RobotTrajectoryPtr& result,
156+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
157157

158158
rclcpp::Node::SharedPtr node_;
159159

‎core/include/moveit/task_constructor/solvers/planner_interface.h‎

Lines changed: 17 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -71,6 +71,14 @@ class PlannerInterface
7171
PropertyMap properties_;
7272

7373
public:
74+
struct Result
75+
{
76+
bool success;
77+
std::string message;
78+
79+
operator bool() const { return success; }
80+
};
81+
7482
PlannerInterface();
7583
virtual ~PlannerInterface() {}
7684

@@ -88,17 +96,17 @@ class PlannerInterface
8896
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
8997

9098
/// plan trajectory between to robot states
91-
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
92-
const moveit::core::JointModelGroup* jmg, double timeout,
93-
robot_trajectory::RobotTrajectoryPtr& result,
94-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
99+
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from,
100+
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
101+
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
102+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
95103

96104
/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
97-
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
98-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
99-
const moveit::core::JointModelGroup* jmg, double timeout,
100-
robot_trajectory::RobotTrajectoryPtr& result,
101-
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
105+
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
106+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
107+
const moveit::core::JointModelGroup* jmg, double timeout,
108+
robot_trajectory::RobotTrajectoryPtr& result,
109+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
102110

103111
// get name of the planner
104112
virtual std::string getPlannerId() const = 0;

0 commit comments

Comments
 (0)