From 060c90c94a422d7c8be8e9866b61539d3cd2276e Mon Sep 17 00:00:00 2001
From: Erdal Pekel <info@erdalpekel.de>
Date: Fri, 1 Mar 2019 14:26:07 -0800
Subject: [PATCH] MoveIt! - Constraint-aware planning
---
.clang-format | 5 ++
CMakeLists.txt | 180 ++++---------------------------------
README.md | 6 +-
launch/simulation.launch | 3 +
package.xml | 37 ++------
src/robot_control_node.cpp | 124 +++++++++++++++++++++++++
6 files changed, 159 insertions(+), 196 deletions(-)
create mode 100755 .clang-format
create mode 100644 src/robot_control_node.cpp
diff --git a/.clang-format b/.clang-format
new file mode 100755
index 0000000..67219d9
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,5 @@
+BraceWrapping:
+ AfterFunction: false
+ColumnLimit: 120
+AllowShortBlocksOnASingleLine: true
+AllowShortIfStatementsOnASingleLine: false
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8161773..3f59073 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 2.8.3)
project(panda_simulation)
## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+add_compile_options(-std=c++14)
+set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}" )
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -19,102 +21,29 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
tf
xacro
+ moveit_core
+ moveit_visual_tools
+ moveit_ros_planning
+ moveit_ros_planning_interface
)
## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Boost REQUIRED COMPONENTS filesystem)
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-## * add a build_depend tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-## * add "message_generation" and every package in MSG_DEP_SET to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * add "message_runtime" and every package in MSG_DEP_SET to
-## catkin_package(CATKIN_DEPENDS ...)
-## * uncomment the add_*_files sections below as needed
-## and list every .msg/.srv/.action file to be processed
-## * uncomment the generate_messages entry below
-## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-# FILES
-# Action1.action
-# Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(JSONCPP jsoncpp)
+message(${JSONCPP_LIBRARIES})
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
-# INCLUDE_DIRS include
-# LIBRARIES panda_simulation
-# CATKIN_DEPENDS controller_manager effort_controllers gazebo_ros joint_state_controller joint_state_publisher joint_trajectory_controller robot_state_publisher roscpp std_msgs tf xacro
-# DEPENDS system_lib
+ CATKIN_DEPENDS
+ moveit_core
+ moveit_visual_tools
+ moveit_ros_planning_interface
)
###########
@@ -124,84 +53,9 @@ catkin_package(
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
-# include
${catkin_INCLUDE_DIRS}
)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/panda_simulation.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/panda_simulation_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-# FILES_MATCHING PATTERN "*.h"
-# PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-# # myfile1
-# # myfile2
-# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_panda_simulation.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+add_executable(robot_control_node src/robot_control_node.cpp)
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
\ No newline at end of file
diff --git a/README.md b/README.md
index 6f382f1..a0dca69 100644
--- a/README.md
+++ b/README.md
@@ -3,7 +3,7 @@
This package was built for ROS melodic running under Ubuntu 18.04. Run the following command to make sure that all additional packages are installed:
```
-sudo apt-get install ros-melodic-moveit-ros-move-group ros-melodic-controller-manager* ros-melodic-moveit* ros-melodic-effort-controllers ros-melodic-joint-trajectory-controller ros-melodic-gazebo-ros* ros-melodic-rviz*
+sudo apt-get install ros-melodic-moveit-ros-move-group ros-melodic-controller-manager* ros-melodic-moveit* ros-melodic-effort-controllers ros-melodic-joint-trajectory-controller ros-melodic-gazebo-ros* ros-melodic-rviz* libboost-filesystem-dev libjsoncpp-dev
```
It is also important that you build the *libfranka* library from source and pass its directory to *catkin_make* when building this ROS package as described in [this tutorial](https://frankaemika.github.io/docs/installation.html#building-from-source).
@@ -25,3 +25,7 @@ Otherwise, the robot will appear in rviz in a collapsed state.
You can see the full explanation in my [blog post](https://erdalpekel.de/?p=55).
+
+## Extension: _MoveIt!_ constraint-aware planning
+
+This repository was extended with a _MoveIt!_ planning where the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_simulation`) as _JSON_ files. You can read more about it in the accompanying [blog post](https://erdalpekel.de/?p=123).
diff --git a/launch/simulation.launch b/launch/simulation.launch
index 1bfdc68..73b3197 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -39,5 +39,8 @@
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
+
+ <!-- launch robot control node for moveit motion planning -->
+ <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" />
</launch>
\ No newline at end of file
diff --git a/package.xml b/package.xml
index d8cb1f9..74a2bf0 100644
--- a/package.xml
+++ b/package.xml
@@ -16,38 +16,6 @@
<license>TODO</license>
- <!-- Url tags are optional, but multiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/panda_simulation</url> -->
-
-
- <!-- Author tags are optional, multiple are allowed, one per tag -->
- <!-- Authors do not have to be maintainers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
- <!-- The *depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
- <!-- <depend>roscpp</depend> -->
- <!-- Note that this is equivalent to the following: -->
- <!-- <build_depend>roscpp</build_depend> -->
- <!-- <exec_depend>roscpp</exec_depend> -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use build_export_depend for packages you need in order to build against this package: -->
- <!-- <build_export_depend>message_generation</build_export_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use exec_depend for packages you need at runtime: -->
- <!-- <exec_depend>message_runtime</exec_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <!-- Use doc_depend for packages you need only for building documentation: -->
- <!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>controller_manager</build_depend>
<build_depend>effort_controllers</build_depend>
@@ -83,6 +51,11 @@
<exec_depend>tf</exec_depend>
<exec_depend>xacro</exec_depend>
+ <!-- CUSTOM -->
+ <depend>moveit_core</depend>
+ <depend>moveit_ros_planning_interface</depend>
+ <depend>moveit_perception</depend>
+ <depend>moveit_visual_tools</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/src/robot_control_node.cpp b/src/robot_control_node.cpp
new file mode 100644
index 0000000..e1f857b
--- /dev/null
+++ b/src/robot_control_node.cpp
@@ -0,0 +1,124 @@
+#include <jsoncpp/json/json.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <ros/ros.h>
+#include <boost/filesystem.hpp>
+
+static const std::string PLANNING_GROUP_ARM = "panda_arm";
+static const std::string APP_DIRECTORY_NAME = ".panda_simulation";
+
+moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)
+{
+ moveit_msgs::CollisionObject collision_object;
+ collision_object.header.frame_id = "world";
+ collision_object.id = name;
+
+ const Json::Value dimensions = root["dimensions"];
+ ROS_INFO_STREAM("Extracted dimensions: " << dimensions);
+ // Define a box to add to the world.
+ shape_msgs::SolidPrimitive primitive;
+ primitive.type = primitive.BOX;
+ primitive.dimensions.resize(3);
+ primitive.dimensions[0] = dimensions["x"].asDouble();
+ primitive.dimensions[1] = dimensions["y"].asDouble();
+ primitive.dimensions[2] = dimensions["z"].asDouble();
+
+ const Json::Value position = root["position"];
+ ROS_INFO_STREAM("Extracted position: " << position);
+
+ const Json::Value orientation = root["orientation"];
+ ROS_INFO_STREAM("Extracted orientation: " << orientation);
+ // Define a pose for the box (specified relative to frame_id)
+ geometry_msgs::Pose box_pose;
+ box_pose.orientation.w = orientation["w"].asDouble();
+ box_pose.orientation.x = orientation["x"].asDouble();
+ box_pose.orientation.y = orientation["y"].asDouble();
+ box_pose.orientation.z = orientation["z"].asDouble();
+
+ // MoveIt! planning scene expects the center of the object as position.
+ // We add half of its dimension to its position
+ box_pose.position.x = position["x"].asDouble() + primitive.dimensions[0] / 2.0;
+ box_pose.position.y = position["y"].asDouble() + primitive.dimensions[1] / 2.0;
+ box_pose.position.z = position["z"].asDouble() + primitive.dimensions[2] / 2.0;
+
+ collision_object.primitives.push_back(primitive);
+ collision_object.primitive_poses.push_back(box_pose);
+ collision_object.operation = collision_object.ADD;
+
+ return std::move(collision_object);
+}
+
+int main(int argc, char **argv)
+{
+ namespace fs = boost::filesystem;
+ ROS_INFO("RUNNING robot_control_node");
+
+ ros::init(argc, argv, "robot_control_node");
+
+ ros::NodeHandle node_handle;
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+
+ moveit::planning_interface::MoveGroupInterface move_group_arm(PLANNING_GROUP_ARM);
+
+ ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
+ ros::WallDuration sleep_t(0.5);
+ while (planning_scene_diff_publisher.getNumSubscribers() < 1)
+ {
+ sleep_t.sleep();
+ }
+ moveit_msgs::PlanningScene planning_scene;
+
+ // read JSON files from ~/.panda_simulation
+ fs::path home(getenv("HOME"));
+ if (fs::is_directory(home))
+ {
+ fs::path app_directory(home);
+ app_directory /= APP_DIRECTORY_NAME;
+
+ if (!fs::exists(app_directory) && !fs::is_directory(app_directory))
+ {
+ ROS_ERROR_STREAM(app_directory << " does not exist");
+ return -1;
+ }
+
+ std::vector<moveit_msgs::CollisionObject> collision_objects;
+ ROS_INFO_STREAM(app_directory << " is a directory containing:");
+ for (auto &entry : boost::make_iterator_range(fs::directory_iterator(app_directory), {}))
+ {
+ ROS_INFO_STREAM(entry);
+
+ std::ifstream file_stream(entry.path().string(), std::ifstream::binary);
+ if (file_stream)
+ {
+ Json::Value root;
+ file_stream >> root;
+
+ moveit_msgs::CollisionObject collision_object = extractObstacleFromJson(root, entry.path().stem().string());
+ collision_objects.push_back(collision_object);
+ }
+ else
+ {
+ ROS_WARN_STREAM("could not open file " << entry.path());
+ }
+ }
+
+ // Publish the collision objects to the scene
+ for (const auto &collision_object : collision_objects)
+ {
+ collision_object.header.frame_id = move_group_arm.getPlanningFrame();
+ planning_scene.world.collision_objects.push_back(collision_object);
+ }
+
+ ROS_INFO_STREAM("# collision objects " << planning_scene.world.collision_objects.size());
+ planning_scene.is_diff = true;
+ planning_scene_diff_publisher.publish(planning_scene);
+
+ ROS_INFO("robot_control_node is ready");
+ ros::waitForShutdown();
+
+ return 0;
+ }
+}
\ No newline at end of file
--
GitLab