Skip to content

Commit ff4eb4e

Browse files
v4hnv4hn
authored
Run pick+place demo as a unit test (#254)
1 parent 059a939 commit ff4eb4e

8 files changed

Lines changed: 213 additions & 127 deletions

File tree

‎demo/CMakeLists.txt‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,9 @@ find_package(catkin REQUIRED COMPONENTS
1111
rosparam_shortcuts
1212
)
1313

14-
catkin_package()
14+
catkin_package(
15+
CATKIN_DEPENDS roscpp
16+
)
1517

1618
include_directories(
1719
include
@@ -24,7 +26,9 @@ target_link_libraries(cartesian ${catkin_LIBRARIES})
2426
add_executable(modular src/modular.cpp)
2527
target_link_libraries(modular ${catkin_LIBRARIES})
2628

27-
add_executable(pick_place_demo src/pick_place_demo.cpp src/pick_place_task.cpp)
29+
add_library(pick_place_task OBJECT src/pick_place_task.cpp)
30+
31+
add_executable(pick_place_demo src/pick_place_demo.cpp $<TARGET_OBJECTS:pick_place_task>)
2832
target_link_libraries(pick_place_demo ${catkin_LIBRARIES})
2933

3034
add_executable(alternative_path_costs src/alternative_path_costs.cpp)
@@ -42,3 +46,5 @@ install(TARGETS cartesian modular pick_place_demo
4246
install(DIRECTORY launch config
4347
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
4448
)
49+
50+
add_subdirectory(test)

‎demo/include/moveit_task_constructor_demo/pick_place_task.h‎

Lines changed: 10 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -58,32 +58,34 @@
5858
#include <moveit/task_constructor/solvers/pipeline_planner.h>
5959
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
6060

61-
#include <actionlib/client/simple_action_client.h>
62-
#include <actionlib/server/simple_action_server.h>
63-
6461
#include <eigen_conversions/eigen_msg.h>
6562

6663
#pragma once
6764

6865
namespace moveit_task_constructor_demo {
6966
using namespace moveit::task_constructor;
7067

68+
// prepare a demo environment from ROS parameters under pnh
69+
void setupDemoScene(ros::NodeHandle& pnh);
70+
7171
class PickPlaceTask
7272
{
7373
public:
74-
PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh);
74+
PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh);
7575
~PickPlaceTask() = default;
7676

77-
void loadParameters();
78-
79-
void init();
77+
bool init();
8078

8179
bool plan();
8280

8381
bool execute();
8482

8583
private:
86-
ros::NodeHandle nh_;
84+
void loadParameters();
85+
86+
static constexpr char LOGNAME[]{ "pick_place_task" };
87+
88+
ros::NodeHandle pnh_;
8789

8890
std::string task_name_;
8991
moveit::task_constructor::TaskPtr task_;
@@ -107,9 +109,6 @@ class PickPlaceTask
107109
std::string hand_close_pose_;
108110
std::string arm_home_pose_;
109111

110-
// Execution
111-
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> execute_;
112-
113112
// Pick metrics
114113
Eigen::Isometry3d grasp_frame_transform_;
115114
double approach_object_min_dist_;

‎demo/package.xml‎

Lines changed: 11 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -11,10 +11,17 @@
1111
<license>BSD</license>
1212

1313
<buildtool_depend>catkin</buildtool_depend>
14-
<depend>moveit_task_constructor_core</depend>
15-
<depend>moveit_ros_planning_interface</depend>
16-
<depend>moveit_core</depend>
14+
15+
<depend>roscpp</depend>
1716
<depend>rosparam_shortcuts</depend>
18-
<build_depend>roscpp</build_depend>
17+
18+
<depend>moveit_core</depend>
19+
<depend>moveit_ros_planning_interface</depend>
20+
21+
<depend>moveit_task_constructor_core</depend>
22+
23+
<exec_depend>moveit_task_constructor_capabilities</exec_depend>
1924
<exec_depend>moveit_resources_panda_moveit_config</exec_depend>
25+
26+
<test_depend>rostest</test_depend>
2027
</package>

‎demo/src/pick_place_demo.cpp‎

Lines changed: 10 additions & 68 deletions
Original file line numberDiff line numberDiff line change
@@ -40,83 +40,25 @@
4040
// MTC pick/place demo implementation
4141
#include <moveit_task_constructor_demo/pick_place_task.h>
4242

43-
#include <geometry_msgs/Pose.h>
44-
#include <moveit/planning_scene_interface/planning_scene_interface.h>
45-
#include <rosparam_shortcuts/rosparam_shortcuts.h>
46-
#include <tf2_ros/transform_broadcaster.h>
47-
4843
constexpr char LOGNAME[] = "moveit_task_constructor_demo";
4944

50-
void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) {
51-
if (!psi.applyCollisionObject(object))
52-
throw std::runtime_error("Failed to spawn object: " + object.id);
53-
}
54-
55-
moveit_msgs::CollisionObject createTable() {
56-
ros::NodeHandle pnh("~");
57-
std::string table_name, table_reference_frame;
58-
std::vector<double> table_dimensions;
59-
geometry_msgs::Pose pose;
60-
std::size_t errors = 0;
61-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name);
62-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame);
63-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions);
64-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose);
65-
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
66-
67-
moveit_msgs::CollisionObject object;
68-
object.id = table_name;
69-
object.header.frame_id = table_reference_frame;
70-
object.primitives.resize(1);
71-
object.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
72-
object.primitives[0].dimensions = table_dimensions;
73-
pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world
74-
object.primitive_poses.push_back(pose);
75-
return object;
76-
}
77-
78-
moveit_msgs::CollisionObject createObject() {
79-
ros::NodeHandle pnh("~");
80-
std::string object_name, object_reference_frame;
81-
std::vector<double> object_dimensions;
82-
geometry_msgs::Pose pose;
83-
std::size_t error = 0;
84-
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name);
85-
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame);
86-
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions);
87-
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose);
88-
rosparam_shortcuts::shutdownIfError(LOGNAME, error);
89-
90-
moveit_msgs::CollisionObject object;
91-
object.id = object_name;
92-
object.header.frame_id = object_reference_frame;
93-
object.primitives.resize(1);
94-
object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
95-
object.primitives[0].dimensions = object_dimensions;
96-
pose.position.z += 0.5 * object_dimensions[0];
97-
object.primitive_poses.push_back(pose);
98-
return object;
99-
}
100-
10145
int main(int argc, char** argv) {
102-
ROS_INFO_NAMED(LOGNAME, "Init node");
10346
ros::init(argc, argv, "mtc_tutorial");
104-
ros::NodeHandle nh;
47+
ros::NodeHandle nh, pnh("~");
48+
49+
// Handle Task introspection requests from RViz & feedback during execution
10550
ros::AsyncSpinner spinner(1);
10651
spinner.start();
10752

108-
// Add table and object to planning scene
109-
ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service
110-
moveit::planning_interface::PlanningSceneInterface psi;
111-
ros::NodeHandle pnh("~");
112-
if (pnh.param("spawn_table", true))
113-
spawnObject(psi, createTable());
114-
spawnObject(psi, createObject());
53+
moveit_task_constructor_demo::setupDemoScene(pnh);
11554

11655
// Construct and run pick/place task
117-
moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh);
118-
pick_place_task.loadParameters();
119-
pick_place_task.init();
56+
moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh);
57+
if (!pick_place_task.init()) {
58+
ROS_INFO_NAMED(LOGNAME, "Initialization failed");
59+
return 1;
60+
}
61+
12062
if (pick_place_task.plan()) {
12163
ROS_INFO_NAMED(LOGNAME, "Planning succeded");
12264
if (pnh.param("execute", false)) {

‎demo/src/pick_place_task.cpp‎

Lines changed: 108 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -38,9 +38,72 @@
3838
#include <rosparam_shortcuts/rosparam_shortcuts.h>
3939

4040
namespace moveit_task_constructor_demo {
41-
constexpr char LOGNAME[] = "pick_place_task";
42-
PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh)
43-
: nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) {}
41+
42+
constexpr char LOGNAME[] = "moveit_task_constructor_demo";
43+
constexpr char PickPlaceTask::LOGNAME[];
44+
45+
void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) {
46+
if (!psi.applyCollisionObject(object))
47+
throw std::runtime_error("Failed to spawn object: " + object.id);
48+
}
49+
50+
moveit_msgs::CollisionObject createTable(ros::NodeHandle& pnh) {
51+
std::string table_name, table_reference_frame;
52+
std::vector<double> table_dimensions;
53+
geometry_msgs::Pose pose;
54+
std::size_t errors = 0;
55+
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name);
56+
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame);
57+
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions);
58+
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose);
59+
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
60+
61+
moveit_msgs::CollisionObject object;
62+
object.id = table_name;
63+
object.header.frame_id = table_reference_frame;
64+
object.primitives.resize(1);
65+
object.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
66+
object.primitives[0].dimensions = table_dimensions;
67+
pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world
68+
object.primitive_poses.push_back(pose);
69+
return object;
70+
}
71+
72+
moveit_msgs::CollisionObject createObject(ros::NodeHandle& pnh) {
73+
std::string object_name, object_reference_frame;
74+
std::vector<double> object_dimensions;
75+
geometry_msgs::Pose pose;
76+
std::size_t error = 0;
77+
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name);
78+
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame);
79+
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions);
80+
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose);
81+
rosparam_shortcuts::shutdownIfError(LOGNAME, error);
82+
83+
moveit_msgs::CollisionObject object;
84+
object.id = object_name;
85+
object.header.frame_id = object_reference_frame;
86+
object.primitives.resize(1);
87+
object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
88+
object.primitives[0].dimensions = object_dimensions;
89+
pose.position.z += 0.5 * object_dimensions[0];
90+
object.primitive_poses.push_back(pose);
91+
return object;
92+
}
93+
94+
void setupDemoScene(ros::NodeHandle& pnh) {
95+
// Add table and object to planning scene
96+
ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service
97+
moveit::planning_interface::PlanningSceneInterface psi;
98+
if (pnh.param("spawn_table", true))
99+
spawnObject(psi, createTable(pnh));
100+
spawnObject(psi, createObject(pnh));
101+
}
102+
103+
PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh)
104+
: pnh_(pnh), task_name_(task_name) {
105+
loadParameters();
106+
}
44107

45108
void PickPlaceTask::loadParameters() {
46109
/****************************************************
@@ -49,45 +112,44 @@ void PickPlaceTask::loadParameters() {
49112
* *
50113
***************************************************/
51114
ROS_INFO_NAMED(LOGNAME, "Loading task parameters");
52-
ros::NodeHandle pnh("~");
53115

54116
// Planning group properties
55117
size_t errors = 0;
56-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_);
57-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_);
58-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_);
59-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_);
60-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_);
61-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_);
118+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_);
119+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_);
120+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_);
121+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_);
122+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_);
123+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_);
62124

63125
// Predefined pose targets
64-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_);
65-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_);
66-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_);
126+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_);
127+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_);
128+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_);
67129

68130
// Target object
69-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_);
70-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_);
71-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_);
72-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_);
131+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_);
132+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_);
133+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_);
134+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_);
73135
support_surfaces_ = { surface_link_ };
74136

75137
// Pick/Place metrics
76-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_);
77-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_);
78-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_);
79-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_);
80-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_);
81-
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_);
138+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_min_dist", approach_object_min_dist_);
139+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_max_dist", approach_object_max_dist_);
140+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_min_dist", lift_object_min_dist_);
141+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_max_dist", lift_object_max_dist_);
142+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_);
143+
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_);
82144
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
83145
}
84146

85-
void PickPlaceTask::init() {
147+
bool PickPlaceTask::init() {
86148
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
87149
const std::string object = object_name_;
88150

89151
// Reset ROS introspection before constructing the new object
90-
// TODO(henningkayser): verify this is a bug, fix if possible
152+
// TODO(v4hn): global storage for Introspection services to enable one-liner
91153
task_.reset();
92154
task_.reset(new moveit::task_constructor::Task());
93155

@@ -414,36 +476,40 @@ void PickPlaceTask::init() {
414476
stage->restrictDirection(stages::MoveTo::FORWARD);
415477
t.add(std::move(stage));
416478
}
417-
}
418-
419-
bool PickPlaceTask::plan() {
420-
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
421-
ros::NodeHandle pnh("~");
422-
int max_solutions = pnh.param<int>("max_solutions", 10);
423479

480+
// prepare Task structure for planning
424481
try {
425-
task_->plan(max_solutions);
482+
t.init();
426483
} catch (InitStageException& e) {
427484
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
428485
return false;
429486
}
430-
if (task_->numSolutions() == 0) {
431-
ROS_ERROR_NAMED(LOGNAME, "Planning failed");
432-
return false;
433-
}
487+
434488
return true;
435489
}
436490

491+
bool PickPlaceTask::plan() {
492+
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
493+
int max_solutions = pnh_.param<int>("max_solutions", 10);
494+
495+
return task_->plan(max_solutions);
496+
}
497+
437498
bool PickPlaceTask::execute() {
438499
ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory");
439-
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal;
440-
task_->solutions().front()->fillMessage(execute_goal.solution);
441-
execute_.sendGoal(execute_goal);
442-
execute_.waitForResult();
443-
moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code;
500+
moveit_msgs::MoveItErrorCodes execute_result;
501+
502+
execute_result = task_->execute(*task_->solutions().front());
503+
// // If you want to inspect the goal message, use this instead:
504+
// actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>
505+
// execute("execute_task_solution", true); execute.waitForServer();
506+
// moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal;
507+
// task_->solutions().front()->fillMessage(execute_goal.solution);
508+
// execute.sendGoalAndWait(execute_goal);
509+
// execute_result = execute.getResult()->error_code;
444510

445511
if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
446-
ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_.getState().toString());
512+
ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val);
447513
return false;
448514
}
449515

0 commit comments

Comments
 (0)