Compare revisions

        joint_trajectory_controller

        robot_state_publisher

        roscpp

        tf2

        tf2_geometry_msgs

        std_msgs

        xacro

        moveit_core

        controller_interface

        hardware_interface

        pluginlib

        tf2

        simulation_util

        DEPENDS

        # system_lib

# ######################################################################################################################

# Specify additional locations of header files Your package locations should be listed before other locations

include_directories(${catkin_INCLUDE_DIRS})

include_directories(src ${catkin_INCLUDE_DIRS})

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(PickPlacePlanner src/PickPlacePlanner.cpp)

add_executable(PickPlacePlanner src/PickPlacePlanner.cpp src/PickPlacePlannerMain.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(PickPlacePlanner ${catkin_LIBRARIES})

#############

## Testing ##

#############

## Add gtest based cpp test target and link libraries

if(CATKIN_ENABLE_TESTING)

    find_package(rostest REQUIRED)

    add_rostest_gtest(tests_grasping test/grasping.test test/test.cpp src/PickPlacePlanner.cpp)

    target_link_libraries(tests_grasping ${catkin_LIBRARIES})

endif()

 No newline at end of file

Please note that all examples must be run in a built catkin workspace from within

a shell with all ROS paths properly set up (i.e., after running `source devel/setup.sh`).

### Planning and Simulation based on RViz

This is an example to use RViz and Gazebo interactively. Motions can be planned and executed from the RViz GUI.

- Command: `roslaunch sample_applications interactive_planner.launch`

- Expected results:

    - RViz window to control the robot

    - Gazebo window to observe the executed robot motion

    - rqt to show log messages

    - The robot does not move automatically, but can be controlled with RViz (cf. the [tutorial](http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html))

### Minimal "Hello World" Example

This example of an execution of a simple motion.

    - A Gazebo window to observe the executed robot motion opens

    - An rqt window to show log messages opens

    - 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

      problems can also happen during an actual execution, so we have to live with them.

    - If a problem like this occurs, be thankful that it was just a simulation and simply restart.

### Planning and Simulation based on RViz

This is an example to use RViz and Gazebo interactively. Motions can be planned and executed from the RViz GUI.

- Command: `roslaunch sample_applications interactive_planner.launch`

- Expected results:

    - RViz window to control the robot

    - Gazebo window to observe the executed robot motion

    - rqt to show log messages

    - The robot does not move automatically, but can be controlled with RViz (cf. the [tutorial](http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html))

    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />

</launch>

    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    <node name="spawn_box" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/box.urdf -model box" respawn="false" output="screen" />

    <node name="spawn_box" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find sample_applications)/models/box.urdf -model box" respawn="false" output="screen" />

    <node pkg="sample_applications" type="ObstacleAwarePlanner" name="ObstacleAwarePlannerInstance" respawn="false" output="screen"/>

    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />

    <node pkg="sample_applications" type="pick_place" name="pick_place" />

    <node pkg="sample_applications" type="PickPlacePlanner" name="PickPlacePlanner" />

</launch>

  <depend>robot_state_publisher</depend>

  <depend>roscpp</depend>

  <depend>std_msgs</depend>

  <depend>tf</depend>

  <depend>xacro</depend>

  <depend>moveit_simple_controller_manager</depend>

  <!-- CUSTOM -->

  <depend>tf2</depend>

  <depend>tf2_geometry_msgs</depend>

  <depend>moveit_core</depend>

  <depend>moveit_ros_planning_interface</depend>

  <depend>moveit_ros_perception</depend>

    static const std::string PLANNING_GROUP = "panda_arm";

    moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);

    // visualization setup

    // MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz

    namespace rvt = rviz_visual_tools;

    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");

    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations

    visual_tools.enableBatchPublishing();

    // remote control is an introspection tool that allows users to step through a high level script

    // via buttons and keyboard shortcuts in RViz

    visual_tools.loadRemoteControl();

    ros::Duration(3.0).sleep();

    // we can use visual_tools to wait for user input

    visual_tools.prompt("Press 'next' to move the simulated robot. \n");

    visual_tools.trigger();

    group.setStartStateToCurrentState();

    // You can plan a Cartesian path directly by specifying a list of waypoints

    rt.getRobotTrajectoryMsg(trajectory_msg);

    */

    visual_tools.trigger();

    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to move the simulated robot");

    // execute the planned motion

    group.execute(plan);

    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    // visualization setup

    // MoveIts visual tools provide capabilities for visualizing objects, robots, and trajectories in RViz

    namespace rvt = rviz_visual_tools;

    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");

    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations

    visual_tools.enableBatchPublishing();

    // remote control is an introspection tool that allows users to step through a high level script

    // via buttons and keyboard shortcuts in RViz

    visual_tools.loadRemoteControl();

    ros::Duration(3.0).sleep();

    // we can use visual_tools to wait for user input

    visual_tools.prompt("Press 'next' to plan the motion. \n");

    visual_tools.trigger();

    // getting basic information

    ROS_INFO("Planning frame: %s", move_group.getPlanningFrame().c_str());

    ROS_INFO("End effector link: %s", move_group.getEndEffectorLink().c_str());

    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ROS_INFO("Visualizing plan %s", success ? "" : "FAILED");

    visual_tools.trigger();

    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

    // Move the (simulated) robot in gazebo

    move_group.move();

#include <moveit/move_group_interface/move_group_interface.h>

// TF2

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

// Zone Spawner to add the collision objects to Gazebo

#include <GazeboZoneSpawner.h>

#include "PickPlacePlanner.h"

namespace pick_place_planner {

/**

 * Sets the posture argument to an open state for the two gripper fingers (8cm apart).

 * @param posture An empty posture that is filled with a posture for two fingers

 */

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

}

/**

 * Pick an object from a fixed position.

 * @param move_group The move group the pick operation is performed on

 */

void pick(moveit::planning_interface::MoveGroupInterface& move_group)

moveit::planning_interface::MoveItErrorCode pick(moveit::planning_interface::MoveGroupInterface& move_group)

{

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

  // your manipulator which in this case would be `"panda_link8"` You will have to compensate for the

  // transform from `"panda_link8"` to the palm of the end effector.

  grasps[0].grasp_pose.header.frame_id = "panda_link0";

  tf2::Quaternion orientation;

  orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);

  // object orientation (default orientation)

  tf2::Quaternion object_orientation{0, 0, 0, 1};

  // gripper orientation

  tf2::Quaternion gripper_orientation;

  // We know the gripper is rotated -45 degrees from the urdf model:

  // The "hand" definition in franka_description/robots/hand.xacro

  //     <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">

  //     <xacro:macro name="hand" params="connected_to:='' ns:='' rpy:='0 0 0' xyz:='0 0 0' ">

  //       <xacro:unless value="${connected_to == ''}">

  //         <joint name="${ns}_hand_joint" type="fixed">

  //           <parent link="${connected_to}"/>

  //           <child link="${ns}_hand"/>

  //           <origin xyz="${xyz}" rpy="${rpy}"/>

  //         </joint>

  //       </xacro:unless>

  //       <!-- other things... -->

  //       </xacro:macro>

  //     </robot>

  //

  // Its use in franka_description/robots/panda_arm_hand.urdf.xacro

  //     <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>

  gripper_orientation.setRPY(0, 0, -M_PI_4);

  // If we did not know it, we could also look it up!

  // tf2_ros::Buffer tfBuffer;

  // tf2_ros::TransformListener tfListener(tfBuffer);

  // ros::WallDuration(0.1).sleep(); // wait a bit for the tf listener

  // tf2::fromMsg((tfBuffer.lookupTransform("panda_hand", "panda_link8", ros::Time(0)).transform).rotation, gripper_orientation);

  // approach orientation

  double approach_roll, approach_pitch, approach_yaw;

  approach_roll =  -M_PI_2; // The neutral rotation has the gripper "looking down",

  approach_pitch = -M_PI_4; // with these transformations, it grasps from one side.

  approach_yaw =   -M_PI_4; // This results in (x=-0.5,y=-0.5,z=0,w=0.7071068)

  tf2::Quaternion approach_orientation;

  approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);

  tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;

  grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);

  grasps[0].grasp_pose.pose.position.x = 0.4;

  grasps[0].grasp_pose.pose.position.y = 0;

  // Set support surface as table1.

  move_group.setSupportSurfaceName("table1");

  // Call pick to pick up the object using the grasps given

  move_group.pick("object", grasps);

  return move_group.pick("object", grasps);

}

/**

 * Place an object at a fixed position on top of table2.

 * @param group

 */

void place(moveit::planning_interface::MoveGroupInterface& group)

moveit::planning_interface::MoveItErrorCode place(moveit::planning_interface::MoveGroupInterface& group)

{

  std::vector<moveit_msgs::PlaceLocation> place_location;

  place_location.resize(1);

  // Set support surface as table2.

  group.setSupportSurfaceName("table2");

  // Call place to place the object using the place locations given.

  group.place("object", place_location);

  return group.place("object", place_location);

}

/**

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

  ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");

  ros::Duration(3.0).sleep();

  ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");

  spinner.start();

} // end namespace pick_place_planner

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

  ros::waitForShutdown();

  return 0;

}

#ifndef SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H

#define SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H

// ROS

#include <ros/ros.h>

// MoveIt

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit/move_group_interface/move_group_interface.h>

namespace pick_place_planner {

/**

 * Sets the posture argument to an open state for the two gripper fingers (8cm apart).

 * @param posture An empty posture that is filled with a posture for two fingers

 */

void openGripper(trajectory_msgs::JointTrajectory &posture);

/**

 * Sets the posture argument to a closed state for the two gripper fingers

 * @param posture An empty posture that is filled with a posture for two fingers

 */

void closedGripper(trajectory_msgs::JointTrajectory &posture);

/**

 * Pick an object from a fixed position.

 * @param move_group The move group the pick operation is performed on

 */

moveit::planning_interface::MoveItErrorCode pick(moveit::planning_interface::MoveGroupInterface &move_group);

/**

* Place an object at a fixed position on top of table2.

* @param group

*/

moveit::planning_interface::MoveItErrorCode place(moveit::planning_interface::MoveGroupInterface &group);

/**

* Add the collision objects to the planning scene and to gazebo.

* @param planning_scene_interface The interface to access the planning scene.

*/

void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface &planning_scene_interface);

}

#endif // SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H

 No newline at end of file

#include "PickPlacePlanner.h"

int main(int argc, char** argv)

{

    ros::init(argc, argv, "panda_arm_pick_place");

    ros::NodeHandle nh;

    ros::AsyncSpinner spinner(1);

    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");

    ros::Duration(3.0).sleep();

    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");

    spinner.start();

    ros::WallDuration(1.0).sleep();

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    moveit::planning_interface::MoveGroupInterface group("panda_arm");

    group.setPlanningTime(45.0);

    pick_place_planner::addCollisionObjects(planning_scene_interface);

    // Wait a bit for ROS things to initialize

    ros::WallDuration(1.0).sleep();

    pick_place_planner::pick(group);

    ros::WallDuration(1.0).sleep();

    pick_place_planner::place(group);

    ros::waitForShutdown();

    return 0;

}

 No newline at end of file

<launch>

    <include file="$(find panda_simulation)/launch/simulation.launch">

        <arg name="slow_simulation" value="false" />

    </include>

    <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"

          args="-d $(find sample_applications)/test/plan_visualizer.rviz" output="screen">

        <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>

    </node>

    <test test-name="PickPlacePlannerTest" pkg="sample_applications" type="tests_grasping"  time-limit="3600.0" />

</launch>

 No newline at end of file

Panels:

  []

Preferences:

  PromptSaveOnExit: true

Toolbars:

  toolButtonStyle: 2

Visualization Manager:

  Class: ""

  Displays:

    - Alpha: 0.5

      Cell Size: 1

      Class: rviz/Grid

      Color: 160; 160; 164

      Enabled: true

      Line Style:

        Line Width: 0.029999999329447746

        Value: Lines

      Name: Grid

      Normal Cell Count: 0

      Offset:

        X: 0

        Y: 0

        Z: 0

      Plane: XY

      Plane Cell Count: 10

      Reference Frame: <Fixed Frame>

      Value: true

    - Acceleration_Scaling_Factor: 1

      Class: moveit_rviz_plugin/MotionPlanning

      Enabled: true

      Move Group Namespace: ""

      MoveIt_Allow_Approximate_IK: false

      MoveIt_Allow_External_Program: false

      MoveIt_Allow_Replanning: false

      MoveIt_Allow_Sensor_Positioning: false

      MoveIt_Goal_Tolerance: 0

      MoveIt_Planning_Attempts: 10

      MoveIt_Planning_Time: 5

      MoveIt_Use_Cartesian_Path: false

      MoveIt_Use_Constraint_Aware_IK: true

      MoveIt_Warehouse_Host: 127.0.0.1

      MoveIt_Warehouse_Port: 33829

      MoveIt_Workspace:

        Center:

          X: 0

          Y: 0

          Z: 0

        Size:

          X: 2

          Y: 2

          Z: 2

      Name: MotionPlanning

      Planned Path:

        Color Enabled: false

        Interrupt Display: false

        Links:

          All Links Enabled: true

          Expand Joint Details: false

          Expand Link Details: false

          Expand Tree: false

          Link Tree Style: Links in Alphabetic Order

          panda_hand:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_leftfinger:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link0:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link1:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link2:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link3:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link4:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link5:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link6:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link7:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link8:

            Alpha: 1

            Show Axes: false

            Show Trail: false

          panda_rightfinger:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          world:

            Alpha: 1

            Show Axes: false

            Show Trail: false

        Loop Animation: true

        Robot Alpha: 0.5

        Robot Color: 150; 50; 150

        Show Robot Collision: false

        Show Robot Visual: true

        Show Trail: false

        State Display Time: 0.05 s

        Trail Step Size: 1

        Trajectory Topic: move_group/display_planned_path

      Planning Metrics:

        Payload: 1

        Show Joint Torques: false

        Show Manipulability: false

        Show Manipulability Index: false

        Show Weight Limit: false

        TextHeight: 0.07999999821186066

      Planning Request:

        Colliding Link Color: 255; 0; 0

        Goal State Alpha: 1

        Goal State Color: 250; 128; 0

        Interactive Marker Size: 0

        Joint Violation Color: 255; 0; 255

        Planning Group: panda_arm

        Query Goal State: false

        Query Start State: false

        Show Workspace: false

        Start State Alpha: 1

        Start State Color: 0; 255; 0

      Planning Scene Topic: move_group/monitored_planning_scene

      Robot Description: robot_description

      Scene Geometry:

        Scene Alpha: 0.8999999761581421

        Scene Color: 50; 230; 50

        Scene Display Time: 0.20000000298023224

        Show Scene Geometry: true

        Voxel Coloring: Z-Axis

        Voxel Rendering: Occupied Voxels

      Scene Robot:

        Attached Body Color: 150; 50; 150

        Links:

          All Links Enabled: true

          Expand Joint Details: false

          Expand Link Details: false

          Expand Tree: false

          Link Tree Style: Links in Alphabetic Order

          panda_hand:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_leftfinger:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link0:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link1:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link2:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link3:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link4:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link5:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link6:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link7:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          panda_link8:

            Alpha: 1

            Show Axes: false

            Show Trail: false

          panda_rightfinger:

            Alpha: 1

            Show Axes: false

            Show Trail: false

            Value: true

          world:

            Alpha: 1

            Show Axes: false

            Show Trail: false

        Robot Alpha: 0.5

        Show Robot Collision: false

        Show Robot Visual: false

      Value: true

      Velocity_Scaling_Factor: 1

  Enabled: true

  Global Options:

    Background Color: 48; 48; 48

    Default Light: true

    Fixed Frame: panda_link0

    Frame Rate: 30

  Name: root

  Tools:

    - Class: rviz/Interact

      Hide Inactive Objects: true

    - Class: rviz/MoveCamera

    - Class: rviz/Select

    - Class: rviz_visual_tools/KeyTool

  Value: true

  Views:

    Current:

      Class: rviz/XYOrbit

      Distance: 2.9803097248077393

      Enable Stereo Rendering:

        Stereo Eye Separation: 0.05999999865889549

        Stereo Focal Distance: 1

        Swap Stereo Eyes: false

        Value: false

      Focal Point:

        X: 1.0864897966384888

        Y: 0.3387787342071533

        Z: 2.2351798634190345e-7

      Focal Shape Fixed Size: true

      Focal Shape Size: 0.05000000074505806

      Invert Z Axis: false

      Name: Current View

      Near Clip Distance: 0.009999999776482582

      Pitch: 0.270203560590744

      Target Frame: panda_link0

      Value: XYOrbit (rviz)

      Yaw: 0.42677485942840576

    Saved: ~

Window Geometry:

  Height: 1404

  Hide Left Dock: false

  Hide Right Dock: false

  MotionPlanning:

    collapsed: false

  MotionPlanning - Trajectory Slider:

    collapsed: false

  QMainWindow State: 000000ff00000000fd00000002000000000000013b000004e1fc0200000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000080000003640000017200fffffffb0000000800480065006c00700100000334000000e10000000000000000fb0000000a00560069006500770073000000003b000004e1000000000000000000000003000004fe0000003ffc0100000001fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000000000004fe0000010000ffffff000004fe000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000

  Width: 1278

  X: 0

  Y: 18

#include "PickPlacePlanner.h"

#include <gtest/gtest.h>

class Grasping : public ::testing::Test {

public:

    Grasping(): spinner(nullptr) {};

protected:

    ros::AsyncSpinner* spinner;

    ros::NodeHandle* node;

    void SetUp() override {

        ::testing::Test::SetUp();

        this->node = new ros::NodeHandle("~");

        this->spinner = new ros::AsyncSpinner(2);

        this->spinner->start();

        ROS_INFO(">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");

        ros::Duration(3.0).sleep();

        ROS_INFO(">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");

        // clean up scene

        CleanupScene();

    }

    static void CleanupScene() {

        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

        for (auto p : planning_scene_interface.getObjects()) {

            p.second.operation = moveit_msgs::CollisionObject::REMOVE;

            planning_scene_interface.applyCollisionObject(p.second);

        }

        ASSERT_EQ(planning_scene_interface.getObjects().size(), 0);

        ASSERT_EQ(planning_scene_interface.getAttachedObjects().size(), 0);

    }

    void TearDown() override