Before attempt to encapsulate launch here

Author: Kai Thouard

include/panda_moveit/pick_and_place.hpp

@@ -39,17 +39,15 @@ namespace pnp
         const std::string PLANNING_GROUP_ARM = "panda_arm";
         const std::string PLANNING_GROUP_GRIPPER = "panda_hand";
 
-        moveit::planning_interface::MoveGroupInterface move_group_interface_arm;
-        moveit::planning_interface::MoveGroupInterface move_group_interface_gripper;
+        std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_interface_arm;
+        std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_interface_gripper;
 
         // Create a moveit::planning_interface::MoveGroupInterface::Plan object to store the movements
         moveit::planning_interface::MoveGroupInterface::Plan plan;
 
         // planning_scene_interface allows us to add and remove collision objects in the world
         moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 
-        const robot_state::JointModelGroup *joint_model_group_arm;
-        const robot_state::JointModelGroup *joint_model_group_gripper;
 
         std::vector<double> floor_dimensions = {2.5, 2.5, 0.01};
         std::vector<double> floor_position = {0.0, 0.0, -0.01};
@@ -154,6 +152,15 @@ namespace pnp
          */
         void go_to_home_position(void);
 
+        /**
+         * @brief Opens the gripper.
+         * 
+         * This function uses MoveIt! to plan and execute a trajectory that moves the gripper from its current position to its open position.
+         * A message is printed to the ROS console indicating whether the trajectory planning was successful or not.
+         * If the plan is successful, the gripper is then moved to the open position.
+         * 
+         * @note This function does not take any parameters and does not return a value.
+        */
         void open_gripper(void);
 
         /**

launch/run_sim.launch

@@ -52,20 +52,20 @@
   </group>
 
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(find franka_gazebo)/launch/move_group.launch" pass_all_args="true">
+  <include file="$(find panda_moveit_config)/launch/move_group.launch" pass_all_args="true">
     <arg name="allow_trajectory_execution" value="true" />
     <arg name="info" value="true" />
   </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(find franka_gazebo)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
+  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
     <arg name="rviz_tutorial" value="$(arg rviz_tutorial)"/>
-    <arg name="rviz_config" value="$(find franka_gazebo)/launch/moveit.rviz"/>
+    <arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
     <arg name="debug" value="$(arg debug)"/>
   </include>
 
   <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(find franka_gazebo)/launch/default_warehouse_db.launch" if="$(arg db)">
+  <include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
     <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
   </include>
 

package.xml

@@ -17,6 +17,7 @@
   <depend>pluginlib</depend>
   <depend>eigen</depend>
   <depend>moveit_core</depend>
+  
   <depend>moveit_ros_planning</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_ros_perception</depend>
@@ -43,10 +44,12 @@
   <depend>rviz</depend>
   <depend>xacro</depend>
   <depend>nodelet</depend>
-  <depend>message_generation</depend>
-  <depend>message_runtime</depend>
   <depend>geometry_msgs</depend>
 
+
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
   <export>
     <!-- Other tools can request additional information be placed here -->
   </export>

src/pick_and_place.cpp

@@ -4,48 +4,48 @@
 // Define the namespace to group related code together
 namespace pnp
 {
-    PickandPlace::PickandPlace(ros::NodeHandle &nh) : move_group_interface_arm(PLANNING_GROUP_ARM), move_group_interface_gripper(PLANNING_GROUP_GRIPPER)
-    {
+    PickandPlace::PickandPlace(ros::NodeHandle &nh)
+    {   
         // Set up a publisher for the pose point visualization marker
         pose_point_pub = nh.advertise<visualization_msgs::Marker>("pose_point", 10);
 
+        move_group_interface_arm = std::make_unique<moveit::planning_interface::MoveGroupInterface>(PLANNING_GROUP_ARM);
+        move_group_interface_gripper = std::make_unique<moveit::planning_interface::MoveGroupInterface>(PLANNING_GROUP_GRIPPER);
+
         // Set the planning time for the arm movement
         const double PLANNING_TIME = 15.0;
-        move_group_interface_arm.setPlanningTime(PLANNING_TIME);
+        move_group_interface_arm->setPlanningTime(PLANNING_TIME);
 
-        // Use raw pointers for performance. These point to the joint model groups for the arm and gripper
-        joint_model_group_arm = move_group_interface_arm.getCurrentState()->getJointModelGroup(PLANNING_GROUP_ARM);
-        joint_model_group_gripper = move_group_interface_gripper.getCurrentState()->getJointModelGroup(PLANNING_GROUP_GRIPPER);
     }
 
     void PickandPlace::writeRobotDetails()
     {
         // Print out the planning frame for the arm
-        ROS_INFO_NAMED("pnp", "Planning frame: %s", move_group_interface_arm.getPlanningFrame().c_str());
+        ROS_INFO_NAMED("pnp", "Planning frame: %s", move_group_interface_arm->getPlanningFrame().c_str());
 
         // Get the link names for the arm and concatenate them into a single string, then print
-        std::vector<std::string> linkNames = move_group_interface_arm.getLinkNames();
+        std::vector<std::string> linkNames = move_group_interface_arm->getLinkNames();
         std::string linkNamesArm = boost::algorithm::join(linkNames, ", ");
         ROS_INFO_NAMED("pnp", "Arm links: %s", linkNamesArm.c_str());
 
         // Get the joint names for the arm and concatenate them into a single string, then print
-        std::vector<std::string> jointNamesArm = move_group_interface_arm.getJoints();
+        std::vector<std::string> jointNamesArm = move_group_interface_arm->getJoints();
         std::string jointNamesArmString = boost::algorithm::join(jointNamesArm, ", ");
         ROS_INFO_NAMED("pnp", "Arm joint names: %s", jointNamesArmString.c_str());
 
         // Get the joint names for the gripper and concatenate them into a single string, then print
-        std::vector<std::string> jointNamesGripper = move_group_interface_gripper.getJoints();
+        std::vector<std::string> jointNamesGripper = move_group_interface_gripper->getJoints();
         std::string jointNamesGripperString = boost::algorithm::join(jointNamesGripper, ", ");
         ROS_INFO_NAMED("pnp", "Gripper joints names: %s", jointNamesGripperString.c_str());
 
         // Get the list of all available planning groups, concatenate them into a single string, then print
-        std::vector<std::string> planningGroups = move_group_interface_arm.getJointModelGroupNames();
+        std::vector<std::string> planningGroups = move_group_interface_arm->getJointModelGroupNames();
         std::string planningGroupsString = boost::algorithm::join(planningGroups, ", ");
         ROS_INFO_NAMED("pnp", "Available Planning Groups: %s", planningGroupsString.c_str());
 
 
         // Get the home joint values and print them out
-        home_joint_values = move_group_interface_arm.getCurrentJointValues();
+        home_joint_values = move_group_interface_arm->getCurrentJointValues();
         std::string jointValuesString;
         for (const auto &joint : home_joint_values)
         {
@@ -184,10 +184,10 @@ namespace pnp
         add_pose_arrow(pose_target.position, rotation_rads[2]);
 
         // set the pose target
-        move_group_interface_arm.setPoseTarget(pose_target);
+        move_group_interface_arm->setPoseTarget(pose_target);
 
         // move to arm to the target pose
-        bool success = (move_group_interface_arm.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+        bool success = (move_group_interface_arm->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
 
         // print if the arm was able to move to the target pose
         ROS_INFO_NAMED("pnp", "Moving to pose target %s", success ? "" : "FAILED");
@@ -254,25 +254,25 @@ namespace pnp
     void PickandPlace::go_to_home_position()
     {
         // uses the home_joint_value variable to go to the home position of the robot
-        move_group_interface_arm.setJointValueTarget(home_joint_values);
+        move_group_interface_arm->setJointValueTarget(home_joint_values);
 
         // move to arm to the target pose
-        bool success = (move_group_interface_arm.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+        bool success = (move_group_interface_arm->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
 
         // print if the arm was able to move to the target pose
         ROS_INFO_NAMED("pnp", "Moving to home position %s", success ? "" : "FAILED");
 
         // move the arm to the target pose
-        move_group_interface_arm.move();
+        move_group_interface_arm->move();
 
     }
 
     void PickandPlace::open_gripper(){
         // Set the joint value target for the gripper
-        move_group_interface_gripper.setJointValueTarget(OPEN_GRIPPER);
-        bool success = (move_group_interface_gripper.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+        move_group_interface_gripper->setJointValueTarget(OPEN_GRIPPER);
+        bool success = (move_group_interface_gripper->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
         ROS_INFO_NAMED("pnp", "Opening gripper %s", success ? "" : "FAILED");
-        move_group_interface_gripper.move();
+        move_group_interface_gripper->move();
 
     }
 
@@ -298,17 +298,17 @@ namespace pnp
         set_pose_target(rod_position, {45, 90, 45});
 
         // execute the plan
-        move_group_interface_arm.move();
+        move_group_interface_arm->move();
 
-        std::cout << "Press enter to continue...";
+        ROS_INFO_NAMED("pnp", "Target Reached - Press any button to continue");
         std::cin.ignore();
 
         // reset
         go_to_home_position();
         clean_scene();
         remove_pose_arrow();
 
-        std::cout << "Press enter to exit...";
+        ROS_INFO_NAMED("pnp", "Simulation Complete - Press any button to exit");
         std::cin.ignore();
 
     }