change to standard interface, introduced sample constraint planner node

CMakeLists.txt

@@ -67,11 +67,11 @@ target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
 add_executable(robot_control_node src/robot_control_node.cpp)
 add_executable(box_publisher_node src/box_publisher.cpp)
 add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
-add_executable(constraint_planner src/constraint_planner.cpp src/SafetyEnvironmentCreator.cpp)
+add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp)
 
 target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
 target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
 target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
-target_link_libraries(constraint_planner ${catkin_LIBRARIES})
+target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES})
 #target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
 

README.md

@@ -38,7 +38,16 @@ You can see the full explanation in my [blog post](https://erdalpekel.de/?p=55).
 
 ## Changelog:
 
-   [_MoveIt!_ constraint-aware planning](https://erdalpekel.de/?p=123)
+   #### Safetyzones
+
+   ![Zone](assets/box.png?raw=true "Safetyzone")
+
+   Integrates safetyzones via the SafetyAwarePlanner. start-command: ... roslaunch panda_simulation simulation.launch ...
+   To see through the boxes one have to set the alpha of the planning an environment (see picture).
+
+   #### _MoveIt!_ constraint-aware planning
+
+   An example of one can add constraints to the planning and simulate it. start-command: start-command: ... roslaunch panda_simulation simulation_constraint.launch ...
 
    This repository was extended with a ROS node that communicates with the _MoveIt!_ Planning Scene API. It makes sure that the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_simulation`) as _json_ files.
 

launch/simulation_constraint.launch

+<launch>
+    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
+
+    <!-- GAZEBO arguments -->
+    <arg name="paused" default="false" />
+    <arg name="use_sim_time" default="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="false" />
+    <arg name="load_gripper" default="true" />
+
+    <!--launch GAZEBO with own world configuration -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
+        <arg name="debug" value="$(arg debug)" />
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="paused" value="$(arg paused)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)" />
+        <arg name="headless" value="$(arg headless)" />
+    </include>
+
+    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
+    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
+
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
+
+    <!-- load the controllers -->
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
+    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
+
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+    <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+        <arg name="load_robot_description" value="true" />
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <include file="$(find panda_moveit_config)/launch/move_group.launch">
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <group if="$(arg gui)">
+        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+    </group>
+
+    <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" output="screen" />
+
+    <!-- load (not start!) custom joint position controller -->
+    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
+
+    <!-- run custom node for automatic intialization -->
+    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
+
+    <node name="SampleConstraintPlanner" pkg="panda_simulation" type="SampleConstraintPlanner" respawn="false" output="screen"/>
+
+</launch>

src/constraint_planner.cpp → src/SafetyAwarePlanner.cpp

@@ -44,8 +44,17 @@ int main(int argc, char** argv)
     std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
               std::ostream_iterator<std::string>(std::cout, ", "));
 
+    shape_msgs::SolidPrimitive safetybox;
+    geometry_msgs::Pose box_pose;
+    safetybox.type = safetybox.BOX;
+    safetybox.dimensions.resize(3);
+
+    safetybox.dimensions[0] = 2.0;
+    safetybox.dimensions[1] = 2.0;
+    safetybox.dimensions[2] = 2.0;
+
     // Safety Box
-    SafetyEnvironmentCreator sec(2.0, 2.0 , 2.0);
+    SafetyEnvironmentCreator sec(safetybox, box_pose);
     std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group);
 
     //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
@@ -69,12 +78,13 @@ int main(int argc, char** argv)
     another_pose.position.z = 0.9;
     move_group.setPoseTarget(another_pose);
 
-
     //moveit_msgs::MotionPlanRequest mr;
     //mr.max_velocity_scaling_factor = 0.5;
     //move_group.constructMotionPlanRequest(mr);
-    move_group.setMaxVelocityScalingFactor(0.1);
-    move_group.setWorkspace(-0.5, -0.5, -0.5, 0, 0, 0);
+
+    //move_group.setMaxVelocityScalingFactor(0.1);
+    //move_group.setWorkspace(-0.5, -0.5, -0.5, 0, 0, 0);
+
     moveit::planning_interface::MoveGroupInterface::Plan my_plan;
     bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
     ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
@@ -84,6 +94,7 @@ int main(int argc, char** argv)
 
     // Move the simulated robot in gazebo
     move_group.move();
+    move_group.stop();
 
     ros::shutdown();
     return 0;

src/SafetyEnvironmentCreator.cpp

@@ -14,11 +14,23 @@ class SafetyEnvironmentCreator {
         double safety_zone_depth;
 
     public:
-        // ctor
-        SafetyEnvironmentCreator(double zone_width, double zone_height, double zone_depth) {
-            safety_zone_width = zone_width;
-            safety_zone_height = zone_height;
-            safety_zone_depth = zone_depth;
+        /**
+         * ctor
+         * @param shape describes the inner dimensions of the safety zone
+         * @param pose describes position and orientation of the safety zone
+         * note: current zone-threshold is 0.1
+         */
+        SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
+            if(shape.type != shape.BOX){
+                ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
+                safety_zone_depth = 0.0;
+                safety_zone_width = 0.0;
+                safety_zone_height = 0.0;
+            }else {
+                safety_zone_depth = shape.dimensions[0];
+                safety_zone_width = shape.dimensions[1];
+                safety_zone_height = shape.dimensions[2];
+            }
         }
 
         double getZoneWidth() {

src/SampleConstraintPlanner.cpp

+//
+// Created by sebastian on 27.03.20.
+//
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+#include <moveit_msgs/DisplayRobotState.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+
+#include <moveit_msgs/AttachedCollisionObject.h>
+#include <moveit_msgs/CollisionObject.h>
+
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "CONSTRAINT PLANNER");
+    ros::NodeHandle node_handle;
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    static const std::string PLANNING_GROUP = "panda_arm";
+    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
+    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+    const robot_state::JointModelGroup* joint_model_group =
+            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
+
+    // Visualization Setup
+    namespace rvt = rviz_visual_tools;
+    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
+    visual_tools.deleteAllMarkers();
+    visual_tools.loadRemoteControl();
+
+    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
+    text_pose.translation().z() = 1.75;
+    visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
+
+    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
+    visual_tools.trigger();
+
+    // Getting Basic Information
+    ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
+    ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
+    ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
+    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
+              std::ostream_iterator<std::string>(std::cout, ", "));
+
+    // Define a collision object ROS message.
+    moveit_msgs::CollisionObject collision_object;
+    collision_object.header.frame_id = move_group.getPlanningFrame();
+
+    // The id of the object is used to identify it.
+    collision_object.id = "box1";
+
+    // Define a box to add to the world.
+    shape_msgs::SolidPrimitive primitive;
+    primitive.type = primitive.BOX;
+    primitive.dimensions.resize(3);
+    primitive.dimensions[0] = 0.4;
+    primitive.dimensions[1] = 0.1;
+    primitive.dimensions[2] = 0.4;
+
+    // Define a pose for the box (specified relative to frame_id)
+    geometry_msgs::Pose box_pose;
+    box_pose.orientation.w = 1.0;
+    box_pose.position.x = 0.4;
+    box_pose.position.y = -0.2;
+    box_pose.position.z = 1.0;
+
+    collision_object.primitives.push_back(primitive);
+    collision_object.primitive_poses.push_back(box_pose);
+    collision_object.operation = collision_object.ADD;
+
+    std::vector<moveit_msgs::CollisionObject> collision_objects;
+    collision_objects.push_back(collision_object);
+
+    // Now, let's add the collision object into the world
+    ROS_INFO_NAMED("constraint_planner", "Add an object into the world");
+    planning_scene_interface.addCollisionObjects(collision_objects);
+
+    // Show text in RViz of status
+    visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
+    visual_tools.trigger();
+    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
+
+    // Now when we plan a trajectory it will avoid the obstacle
+    move_group.setStartState(*move_group.getCurrentState());
+    geometry_msgs::Pose another_pose;
+    another_pose.orientation.w = 1.0;
+    another_pose.position.x = 0.4;
+    another_pose.position.y = -0.4;
+    another_pose.position.z = 0.9;
+    move_group.setPoseTarget(another_pose);
+
+    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+    ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
+
+    visual_tools.prompt("Press 'next' to move the simulated robot.");
+    visual_tools.trigger();
+
+    // Move the simulated robot in gazebo
+    move_group.move();
+
+    ros::shutdown();
+    return 0;
+}
\ No newline at end of file