Error while grasp planning using MoveIt with collision objects originally spawned into gazebo world

edit

Hello, I am facing issues while trying to manipulate objects with the c++ moveit API, which were spawned in the gazebo simulation and published to the planning scene. For this I am using a c++ gazebo plugin "gazebo_ros_moveit_planning_scene.cpp" originally supported by ROS kinetic, which I got from this repository. (https://github.com/ros-simulation/gaz...)

I configured the CMakeLists.txt files accordingly so that the plugin compiles correctly and I launch the plugin by adding it in the "panda_arm_hand.urdf.xacro" file:

<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
  ...
  <gazebo>
  <plugin filename="libgazebo_ros_moveit_planning_scene.so" name="gazebo_ros_moveit_planning_scene">
    <topicName>/planning_scene</topicName>
    <sceneName>(noname)+</sceneName>
    <robotName>panda</robotName>
    <updatePeriod>0.5</updatePeriod>
  </plugin>
  </gazebo>
</robot>

My goal was to recreate the Moveit C++ pick and place tutorial found here Moveit C++ Pick-and-Place However, now the object isn't being created in the MoveIt planning scene directly. Instead, I use C++ code to spawn gazebo objects which will then automatically be published to the /planning_scene by the gazebo plugin.

This seems to work to a certain extent, seeing as the objects are visible in Gazebo as well as in the Rviz-MoveIt MotionPlanning-PlanningScene (image)

When running the code however the following ROS Error gets raised:

ERROR /move_group [/home/tim/ROS_Workspaces/ws_moveit/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:1850(PlanningScene::processCollisionObjectMove)] World object 'object.link' does not exist. Cannot move.

This is the function that executes the grasp planning action and results in the Error.

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;

// 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.x = 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;

// Setting posture of eef before grasp
// +++++++++++++++++++++++++++++++++++
openGripper(grasps[0 ...