moveit_task_constructor/demo/src/pick_place_demo.cpp

136 lines
5.4 KiB
C++

/*********************************************************************
* BSD 3-Clause License
*
* Copyright (c) 2019 PickNik LLC.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Henning Kayser, Simon Goldstein
Desc: A demo to show MoveIt Task Constructor in action
*/
// ROS
#include <ros/ros.h>
// MTC pick/place demo implementation
#include <moveit_task_constructor_demo/pick_place_task.h>
#include <geometry_msgs/Pose.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <rosparam_shortcuts/rosparam_shortcuts.h>
#include <tf2_ros/transform_broadcaster.h>
constexpr char LOGNAME[] = "moveit_task_constructor_demo";
void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) {
if (!psi.applyCollisionObject(object))
throw std::runtime_error("Failed to spawn object: " + object.id);
}
moveit_msgs::CollisionObject createTable() {
ros::NodeHandle pnh("~");
std::string table_name, table_reference_frame;
std::vector<double> table_dimensions;
geometry_msgs::Pose pose;
std::size_t errors = 0;
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose);
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
moveit_msgs::CollisionObject object;
object.id = table_name;
object.header.frame_id = table_reference_frame;
object.primitives.resize(1);
object.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
object.primitives[0].dimensions = table_dimensions;
pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world
object.primitive_poses.push_back(pose);
return object;
}
moveit_msgs::CollisionObject createObject() {
ros::NodeHandle pnh("~");
std::string object_name, object_reference_frame;
std::vector<double> object_dimensions;
geometry_msgs::Pose pose;
std::size_t error = 0;
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name);
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame);
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions);
error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose);
rosparam_shortcuts::shutdownIfError(LOGNAME, error);
moveit_msgs::CollisionObject object;
object.id = object_name;
object.header.frame_id = object_reference_frame;
object.primitives.resize(1);
object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
object.primitives[0].dimensions = object_dimensions;
pose.position.z += 0.5 * object_dimensions[0];
object.primitive_poses.push_back(pose);
return object;
}
int main(int argc, char** argv) {
ROS_INFO_NAMED(LOGNAME, "Init node");
ros::init(argc, argv, "mtc_tutorial");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
// Add table and object to planning scene
ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service
moveit::planning_interface::PlanningSceneInterface psi;
ros::NodeHandle pnh("~");
if (pnh.param("spawn_table", true))
spawnObject(psi, createTable());
spawnObject(psi, createObject());
// Construct and run pick/place task
moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh);
pick_place_task.loadParameters();
pick_place_task.init();
if (pick_place_task.plan()) {
ROS_INFO_NAMED(LOGNAME, "Planning succeded");
if (pnh.param("execute", false)) {
pick_place_task.execute();
ROS_INFO_NAMED(LOGNAME, "Execution complete");
} else {
ROS_INFO_NAMED(LOGNAME, "Execution disabled");
}
} else {
ROS_INFO_NAMED(LOGNAME, "Planning failed");
}
// Keep introspection alive
ros::waitForShutdown();
return 0;
}