From d898ba2e844722f6ce8346f52fd2e66dc2abf1d4 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Mon, 31 Aug 2020 20:15:13 +0200
Subject: [PATCH] align file names and remove old files

---
 CMakeLists.txt                                |  21 +-
 README.md                                     |   7 +-
 launch/grasp_planner.launch                   |  13 -
 ...tion.launch => interactive_planner.launch} |   0
 launch/pick_place_planner.launch              |   9 +
 launch/pick_place_simulation.launch           |  13 -
 models/box.xacro                              |  40 --
 src/GraspPlanner.cpp                          | 352 ------------------
 ...lace_tutorial.cpp => PickPlacePlanner.cpp} |   0
 9 files changed, 23 insertions(+), 432 deletions(-)
 delete mode 100644 launch/grasp_planner.launch
 rename launch/{simulation.launch => interactive_planner.launch} (100%)
 create mode 100644 launch/pick_place_planner.launch
 delete mode 100644 launch/pick_place_simulation.launch
 delete mode 100644 models/box.xacro
 delete mode 100644 src/GraspPlanner.cpp
 rename src/{pick_place_tutorial.cpp => PickPlacePlanner.cpp} (100%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6d19a2f..45eb39b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,8 +5,8 @@ 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) is used, also find other
-# catkin packages
+# Find catkin macros and libraries if COMPONENTS list like `find_package(catkin REQUIRED COMPONENTS xyz)` is used,
+# also find other catkin packages
 find_package(catkin REQUIRED
         COMPONENTS controller_manager
         effort_controllers
@@ -30,10 +30,11 @@ find_package(catkin REQUIRED
 # System dependencies are found with CMake's conventions
 find_package(PkgConfig REQUIRED)
 
-# ################################################################################################################################
+# ######################################################################################################################
 # catkin specific configuration ##
-# ################################################################################################################################
-# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
+# ######################################################################################################################
+# The catkin_package macro generates cmake config files for your package and declare things to be passed to dependent
+# projects. This has to be done in addition to the find_package command above.
 catkin_package(CATKIN_DEPENDS
         moveit_core
         moveit_visual_tools
@@ -46,9 +47,9 @@ catkin_package(CATKIN_DEPENDS
         # system_lib
         )
 
-# ################################################################################################################################
+# ######################################################################################################################
 # Build ##
-# ################################################################################################################################
+# ######################################################################################################################
 
 # Specify additional locations of header files Your package locations should be listed before other locations
 include_directories(${catkin_INCLUDE_DIRS})
@@ -57,13 +58,11 @@ add_executable(BasicCartesianPlanner src/BasicCartesianPlanner.cpp)
 add_executable(BasicJointSpacePlanner src/BasicJointSpacePlanner.cpp)
 add_executable(ObstacleAwarePlanner src/ObstacleAwarePlanner.cpp)
 add_executable(GraspPlanner src/GraspPlanner.cpp)
-add_executable(${PROJECT_NAME}_pick_place src/pick_place_tutorial.cpp)
-
-set_target_properties(${PROJECT_NAME}_pick_place PROPERTIES OUTPUT_NAME pick_place PREFIX "")
+add_executable(PickPlacePlanner src/PickPlacePlanner.cpp)
 
 ## Add cmake target dependencies of the executable
 target_link_libraries(BasicCartesianPlanner ${catkin_LIBRARIES})
 target_link_libraries(BasicJointSpacePlanner ${catkin_LIBRARIES})
 target_link_libraries(ObstacleAwarePlanner ${catkin_LIBRARIES})
 target_link_libraries(GraspPlanner ${catkin_LIBRARIES})
-target_link_libraries(${PROJECT_NAME}_pick_place ${catkin_LIBRARIES})
+target_link_libraries(PickPlacePlanner ${catkin_LIBRARIES})
diff --git a/README.md b/README.md
index 851da8b..52d1851 100644
--- a/README.md
+++ b/README.md
@@ -51,13 +51,14 @@ In this example, an object can be observed in the RViz planning window. This obj
 - Expected results:
     - A Gazebo window to observe the executed robot motion opens
     - An rqt window to show log messages opens
-    - An RViz window to see the plan and trigger the motion open.
+    - An RViz window to show the plan and trigger the motion open
     - The motion does **not** start automatically, instead it must be triggered by pressing *next* in the RvizVisualToolsGUI within RViz
 
 ### Execution of a Pick-and-Place Task
 
-- Command: `roslaunch sample_applications pick_place_simulation.launch`
+- Command: `roslaunch sample_applications pick_place_planner.launch`
 - Expected results:
+    - An RViz window to show the pick and place plan
     - A Gazebo window to observe the executed robot motion opens
     - Two tables an a "stick" on top of one of them are spawned in Gazebo
     - The Robot positions itself behind the stick and grasps it from the side.
@@ -75,7 +76,7 @@ In this example, an object can be observed in the RViz planning window. This obj
 
 This is an example to use RViz and Gazebo interactively. Motions can be planned and executed from the RViz GUI.
 
-- Command: `roslaunch sample_applications simulation.launch`
+- Command: `roslaunch sample_applications interactive_planner.launch`
 - Expected results:
     - RViz window to control the robot
     - Gazebo window to observe the executed robot motion
diff --git a/launch/grasp_planner.launch b/launch/grasp_planner.launch
deleted file mode 100644
index e1cbf6c..0000000
--- a/launch/grasp_planner.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<launch>
-
-    <include file="$(find panda_simulation)/launch/simulation.launch"/>
-
-    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
-
-    <node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/object.urdf -model c_object" respawn="false" output="screen" />
-    <node name="spawn_table1" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/table1.urdf -model c_table1" respawn="false" output="screen" />
-    <node name="spawn_table2" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/table2.urdf -model c_table2" respawn="false" output="screen" />
-
-    <node pkg="sample_applications" type="GraspPlanner" name="GraspPlannerInstance" respawn="false" output="screen"/>
-
-</launch>
diff --git a/launch/simulation.launch b/launch/interactive_planner.launch
similarity index 100%
rename from launch/simulation.launch
rename to launch/interactive_planner.launch
diff --git a/launch/pick_place_planner.launch b/launch/pick_place_planner.launch
new file mode 100644
index 0000000..19c5ca6
--- /dev/null
+++ b/launch/pick_place_planner.launch
@@ -0,0 +1,9 @@
+<launch>
+
+    <include file="$(find panda_simulation)/launch/simulation.launch"/>
+
+    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+
+    <node pkg="sample_applications" type="pick_place" name="pick_place" />
+
+</launch>
diff --git a/launch/pick_place_simulation.launch b/launch/pick_place_simulation.launch
deleted file mode 100644
index 6712abc..0000000
--- a/launch/pick_place_simulation.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-<launch>
-
-    <include file="$(find panda_simulation)/launch/simulation.launch"/>
-
-<!--    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />-->
-
-<!--    <rosparam file="$(find sample_applications)/config/config.yaml" command="load" />-->
-<!--    <node name="safety_zone_spawner" pkg="sample_applications" type="safety_zone_spawner" respawn="false" output="screen"/>-->
-
-    <node pkg="sample_applications" type="pick_place" name="pick_place" />
-<!--    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />-->
-
-</launch>
diff --git a/models/box.xacro b/models/box.xacro
deleted file mode 100644
index 360e023..0000000
--- a/models/box.xacro
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="simple_box">
-    <link name="object_base_link">
-    </link>
-
-    <joint name="object_base_joint" type="fixed">
-        <parent link="object_base_link"/>
-        <child link="object_link"/>
-        <axis xyz="0 0 1" />
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-    </joint>
-
-    <link name="object_link">
-        <inertial>
-            <origin xyz="0.4 -0.2 1.0" />
-            <mass value="1.0" />
-            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
-        </inertial>
-        <visual>
-            <origin xyz="0.4 -0.2 1.0"/>
-            <geometry>
-                <box size="0.4 0.1 0.4" />
-            </geometry>
-        </visual>
-        <collision>
-            <origin xyz="0.4 -0.2 1.0"/>
-            <geometry>
-                <box size="0.4 0.1 0.4" />
-            </geometry>
-        </collision>
-    </link>
-
-    <gazebo reference="object_base_link">
-        <gravity>0</gravity>
-    </gazebo>
-
-    <gazebo reference="object_link">
-        <material>Gazebo/Blue</material>
-    </gazebo>
-</robot>
\ No newline at end of file
diff --git a/src/GraspPlanner.cpp b/src/GraspPlanner.cpp
deleted file mode 100644
index 0eabfe5..0000000
--- a/src/GraspPlanner.cpp
+++ /dev/null
@@ -1,352 +0,0 @@
-// ROS
-#include <ros/ros.h>
-
-// MoveIt!
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-
-// TF2
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-
-geometry_msgs::Quaternion q_pick_object;
-
-geometry_msgs::Pose getTargetPickPose(){
-
-    std::vector<double> co_dim{0.02, 0.02, 0.2};
-    std::vector<double> co_pose{0.5, 0.0, 0.2};
-
-    geometry_msgs::Pose target_pose;
-
-    tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
-    tf2::Quaternion orientation_object(q_pick_object.x, q_pick_object.y, q_pick_object.z, q_pick_object.w);
-
-    tf2::Quaternion orientation = orientation_gripper * orientation_object;
-    orientation.setZ(orientation.getZ() * -1);
-    orientation.setY(orientation.getY() * -1);
-    orientation.setX(orientation.getX() * -1);
-
-    target_pose.orientation  = tf2::toMsg(orientation);
-    target_pose.position.x = co_pose[0];
-    target_pose.position.y = co_pose[1];
-    target_pose.position.z = co_pose[2] + 0.3;
-
-    return target_pose;
-}
-
-void openGripper(trajectory_msgs::JointTrajectory& posture)
-{
-    // BEGIN_SUB_TUTORIAL open_gripper
-    /* Add both finger joints of panda robot. */
-    posture.joint_names.resize(2);
-    posture.joint_names[0] = "panda_finger_joint1";
-    posture.joint_names[1] = "panda_finger_joint2";
-
-    /* Set them as open, wide enough for the object to fit. */
-    posture.points.resize(1);
-    posture.points[0].positions.resize(2);
-    posture.points[0].positions[0] = 0.04;
-    posture.points[0].positions[1] = 0.04;
-    posture.points[0].time_from_start = ros::Duration(0.5);
-    // END_SUB_TUTORIAL
-}
-
-void closedGripper(trajectory_msgs::JointTrajectory& posture)
-{
-    // BEGIN_SUB_TUTORIAL closed_gripper
-    /* Add both finger joints of panda robot. */
-    posture.joint_names.resize(2);
-    posture.joint_names[0] = "panda_finger_joint1";
-    posture.joint_names[1] = "panda_finger_joint2";
-
-    /* Set them as closed. */
-    posture.points.resize(1);
-    posture.points[0].positions.resize(2);
-    posture.points[0].positions[0] = 0.0;
-    posture.points[0].positions[1] = 0.0;
-    std::cout << "pos: " << posture.points[0].positions[0] << std::endl;
-
-    posture.points[0].effort.resize(2);
-    posture.points[0].effort[0] = 10;
-    posture.points[0].effort[1] = 10;
-
-    std::cout << "s:" << posture.points[0].effort.size() << std::endl;
-    posture.points[0].time_from_start = ros::Duration(0.5);
-    // END_SUB_TUTORIAL
-}
-
-void pick(moveit::planning_interface::MoveGroupInterface& move_group)
-{
-    // BEGIN_SUB_TUTORIAL pick1
-    // Create a vector of grasps to be attempted, currently only creating single grasp.
-    // This is essentially useful when using a grasp generator to generate and test multiple grasps.
-    std::vector<moveit_msgs::Grasp> grasps;
-    grasps.resize(1);
-
-    // Setting grasp pose
-    // ++++++++++++++++++++++
-    // This is the pose of panda_link8. |br|
-    // From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length
-    // of the cube). |br|
-    // Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some
-    // extra padding)
-    grasps[0].grasp_pose.header.frame_id = "panda_link0";
-    /*tf2::Quaternion orientation;
-    orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
-    grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
-    grasps[0].grasp_pose.pose.position.x = 0.415;
-    grasps[0].grasp_pose.pose.position.y = 0;
-    grasps[0].grasp_pose.pose.position.z = 0.5;*/
-
-    grasps[0].grasp_pose.pose = getTargetPickPose();
-    // Setting pre-grasp approach
-    // ++++++++++++++++++++++++++
-    /* Defined with respect to frame_id */
-    grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
-    /* Direction is set as positive x axis */
-    grasps[0].pre_grasp_approach.direction.vector.z = -1.0;
-    grasps[0].pre_grasp_approach.min_distance = 0.095;
-    grasps[0].pre_grasp_approach.desired_distance = 0.115;
-
-    // Setting post-grasp retreat
-    // ++++++++++++++++++++++++++
-    /* Defined with respect to frame_id */
-    grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
-    /* Direction is set as positive z axis */
-    grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
-    grasps[0].post_grasp_retreat.min_distance = 0.1;
-    grasps[0].post_grasp_retreat.desired_distance = 0.25;
-    grasps[0].max_contact_force = 12;
-
-    //std::vector<std::string> atos {"c_object", "panda_rightfinger", "panda_leftfinger"};
-
-    //grasps[0].allowed_touch_objects = atos;
-    // Setting posture of eef before grasp
-    // +++++++++++++++++++++++++++++++++++
-    openGripper(grasps[0].pre_grasp_posture);
-    // END_SUB_TUTORIAL
-
-    // BEGIN_SUB_TUTORIAL pick2
-    // Setting posture of eef during grasp
-    // +++++++++++++++++++++++++++++++++++
-    closedGripper(grasps[0].grasp_posture);
-    // END_SUB_TUTORIAL
-
-    // BEGIN_SUB_TUTORIAL pick3
-    // Set support surface as table1.
-    move_group.setSupportSurfaceName("c_table1");
-    // Call pick to pick up the object using the grasps given
-    move_group.setMaxVelocityScalingFactor(0.05);
-    move_group.setMaxAccelerationScalingFactor(0.05);
-    move_group.pick("c_object", grasps);
-    //move_group.attachObject("c_object");
-    // END_SUB_TUTORIAL
-}
-
-void place(moveit::planning_interface::MoveGroupInterface& group)
-{
-    // BEGIN_SUB_TUTORIAL place
-    // TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last
-    // location in
-    // verbose mode." This is a known issue and we are working on fixing it. |br|
-    // Create a vector of placings to be attempted, currently only creating single place location.
-    std::vector<moveit_msgs::PlaceLocation> place_location;
-    place_location.resize(1);
-
-    // Setting place location pose
-    // +++++++++++++++++++++++++++
-    place_location[0].place_pose.header.frame_id = "panda_link0";
-    tf2::Quaternion orientation;
-    orientation.setRPY(0, 0, M_PI / 2);
-    place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
-
-    /* While placing it is the exact location of the center of the object. */
-    place_location[0].place_pose.pose.position.x = 0;
-    place_location[0].place_pose.pose.position.y = 0.5;
-    place_location[0].place_pose.pose.position.z = 0.3;
-
-    // Setting pre-place approach
-    // ++++++++++++++++++++++++++
-    /* Defined with respect to frame_id */
-    place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
-    /* Direction is set as negative z axis */
-    place_location[0].pre_place_approach.direction.vector.z = -1.0;
-    place_location[0].pre_place_approach.min_distance = 0.095;
-    place_location[0].pre_place_approach.desired_distance = 0.115;
-
-    // Setting post-grasp retreat
-    // ++++++++++++++++++++++++++
-    /* Defined with respect to frame_id */
-    place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
-    /* Direction is set as negative y axis */
-    place_location[0].post_place_retreat.direction.vector.z = 1.0;
-    place_location[0].post_place_retreat.min_distance = 0.1;
-    place_location[0].post_place_retreat.desired_distance = 0.25;
-
-    // Setting posture of eef after placing object
-    // +++++++++++++++++++++++++++++++++++++++++++
-    /* Similar to the pick case */
-    openGripper(place_location[0].post_place_posture);
-
-    // Set support surface as table2.
-    group.setSupportSurfaceName("c_table2");
-    // Call place to place the object using the place locations given.
-    group.setMaxVelocityScalingFactor(0.05);
-    group.setMaxAccelerationScalingFactor(0.05);
-    group.place("c_object", place_location);
-    // END_SUB_TUTORIAL
-}
-
-void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)
-{
-    // BEGIN_SUB_TUTORIAL table1
-    //
-    // Creating Environment
-    // ^^^^^^^^^^^^^^^^^^^^
-    // Create vector to hold 4 collision objects.
-
-    std::vector<moveit_msgs::CollisionObject> collision_objects;
-    collision_objects.resize(4);
-
-
-    // Add the first table where the cube will originally be kept.
-    collision_objects[0].id = "c_table1";
-    collision_objects[0].header.frame_id = "panda_link0";
-
-    // Define the primitive and its dimensions.
-    collision_objects[0].primitives.resize(1);
-    collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
-    collision_objects[0].primitives[0].dimensions.resize(3);
-    collision_objects[0].primitives[0].dimensions[0] = 0.2;
-    collision_objects[0].primitives[0].dimensions[1] = 0.4;
-    collision_objects[0].primitives[0].dimensions[2] = 0.2;
-
-    // Define the pose of the table.
-    collision_objects[0].primitive_poses.resize(1);
-    collision_objects[0].primitive_poses[0].position.x = 0.5;
-    collision_objects[0].primitive_poses[0].position.y = 0;
-    collision_objects[0].primitive_poses[0].position.z = 0.1;
-    // END_SUB_TUTORIAL
-
-    collision_objects[0].operation = collision_objects[0].ADD;
-
-    // BEGIN_SUB_TUTORIAL table2
-    // Add the second table where we will be placing the cube.
-    collision_objects[1].id = "c_table2";
-    collision_objects[1].header.frame_id = "panda_link0";
-
-    // Define the primitive and its dimensions.
-    collision_objects[1].primitives.resize(1);
-    collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
-    collision_objects[1].primitives[0].dimensions.resize(3);
-    collision_objects[1].primitives[0].dimensions[0] = 0.4;
-    collision_objects[1].primitives[0].dimensions[1] = 0.2;
-    collision_objects[1].primitives[0].dimensions[2] = 0.2;
-
-    // Define the pose of the table.
-    collision_objects[1].primitive_poses.resize(1);
-    collision_objects[1].primitive_poses[0].position.x = 0;
-    collision_objects[1].primitive_poses[0].position.y = 0.5;
-    collision_objects[1].primitive_poses[0].position.z = 0.1;
-    // END_SUB_TUTORIAL
-
-    collision_objects[1].operation = collision_objects[1].ADD;
-
-    // BEGIN_SUB_TUTORIAL object
-    // Define the object that we will be manipulating
-    collision_objects[2].header.frame_id = "panda_link0";
-    collision_objects[2].id = "c_object";
-
-    //Define the primitive and its dimensions.
-    collision_objects[2].primitives.resize(1);
-    collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
-    collision_objects[2].primitives[0].dimensions.resize(3);
-    collision_objects[2].primitives[0].dimensions[0] = 0.02;
-    collision_objects[2].primitives[0].dimensions[1] = 0.02;
-    collision_objects[2].primitives[0].dimensions[2] = 0.2;
-
-    //Define the pose of the object.
-    collision_objects[2].primitive_poses.resize(1);
-    collision_objects[2].primitive_poses[0].position.x = 0.5;
-    collision_objects[2].primitive_poses[0].position.y = 0;
-    collision_objects[2].primitive_poses[0].position.z = 0.3;
-
-    geometry_msgs::Pose target_pose;
-    tf2::Quaternion orientation;
-    orientation.setRPY(0, 0, 0);
-
-    geometry_msgs::Pose object_pose;
-    collision_objects[2].primitive_poses[0].orientation = tf2::toMsg(orientation);
-    q_pick_object.w = tf2::toMsg(orientation).w;
-    q_pick_object.x = tf2::toMsg(orientation).x;
-    q_pick_object.y = tf2::toMsg(orientation).y;
-    q_pick_object.z = tf2::toMsg(orientation).z;
-    // END_SUB_TUTORIAL
-
-    // the surface
-    collision_objects[2].operation = collision_objects[2].ADD;
-
-    collision_objects[3].header.frame_id = "panda_link0";
-    collision_objects[3].id = "surface";
-
-    // Define the primitive and its dimensions.
-    collision_objects[3].primitives.resize(1);
-    collision_objects[3].primitives[0].type = collision_objects[1].primitives[0].BOX;
-    collision_objects[3].primitives[0].dimensions.resize(3);
-    collision_objects[3].primitives[0].dimensions[0] = 3.0;
-    collision_objects[3].primitives[0].dimensions[1] = 3.0;
-    collision_objects[3].primitives[0].dimensions[2] = 0.1;
-
-    // Define the pose of the object.
-    collision_objects[3].primitive_poses.resize(1);
-    collision_objects[3].primitive_poses[0].position.x = 0;
-    collision_objects[3].primitive_poses[0].position.y = 0;
-    collision_objects[3].primitive_poses[0].position.z = -0.1;
-    // END_SUB_TUTORIAL
-
-    collision_objects[3].operation = collision_objects[3].ADD;
-
-    planning_scene_interface.applyCollisionObjects(collision_objects);
-}
-
-int main(int argc, char** argv)
-{
-    ros::init(argc, argv, "panda_arm_pick_place");
-    ros::NodeHandle nh;
-    ros::AsyncSpinner spinner(1);
-    spinner.start();
-
-    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
-    ros::Duration(5.0).sleep();
-    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
-
-    if(!ros::param::has("max_grasp_approach_velocity"))
-    {
-        ros::param::set("max_grasp_approach_velocity", 0.05);
-        ros::param::set("max_grasp_approach_acceleration", 0.05);
-
-        ros::param::set("max_grasp_transition_velocity", 0.05);
-        ros::param::set("max_grasp_transition_acceleration", 0.05);
-    }
-
-    ros::WallDuration(1.0).sleep();
-    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-    moveit::planning_interface::MoveGroupInterface group("panda_arm");
-    group.setPlanningTime(45.0);
-
-    addCollisionObjects(planning_scene_interface);
-
-    // Wait a bit for ROS things to initialize
-    ros::WallDuration(1.0).sleep();
-
-    pick(group);
-
-    ros::WallDuration(1.0).sleep();
-
-    place(group);
-
-    //std::vector<std::string>objectsToRemove{"object"};
-    //planning_scene_interface.removeCollisionObjects(objectsToRemove);
-
-    ros::waitForShutdown();
-    return 0;
-}
\ No newline at end of file
diff --git a/src/pick_place_tutorial.cpp b/src/PickPlacePlanner.cpp
similarity index 100%
rename from src/pick_place_tutorial.cpp
rename to src/PickPlacePlanner.cpp
-- 
GitLab