MoveIt! - Constraint-aware planning

.clang-format

+BraceWrapping:
+  AfterFunction: false
+ColumnLimit: 120
+AllowShortBlocksOnASingleLine: true
+AllowShortIfStatementsOnASingleLine: false
\ No newline at end of file

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

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).

launch/simulation.launch

@@ -40,4 +40,7 @@
 
     <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

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>

src/robot_control_node.cpp

+#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