Update workcell node with moveit interface use

README.md

@@ -36,5 +36,7 @@ This repository contains practice code of [ros industrial training melodic branc
     <arg name="rviz_config" value="true"/>
     </include>
     ```
-    
+
 - Under [Launch the Planning Environment](https://industrial-training-master.readthedocs.io/en/melodic/_source/session3/Motion-Planning-RVIZ.html#launch-the-planning-environment), running `roslaunch myworkcell_moveit_config myworkcell_planning_execution.launch` file launches rviz without motion planning. The user need to add the motion planning class manually. Created and added a new rviz configuration file to be launched with motion planning in [this commit](https://github.com/Yeshasvitvs/ros-industrial-training/commit/9f1c491c476c4e21c5a0653885eeb891c335cf13)
+
+- Under more to explore point of [Scan-N-Plan Application: Guidance](https://industrial-training-master.readthedocs.io/en/melodic/_source/session4/Motion-Planning-CPP.html#scan-n-plan-application-guidance), you will need to use move group `getCurrentPose()` api call. This may thrown an error if the `asynchronos spinner` is started after the call to  `start()` of ScanNPlan application. 

myworkcell_core/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   tf2_ros
   tf2_geometry_msgs
+  moveit_ros_planning_interface
 )
 
 ## System dependencies are found with CMake's conventions
@@ -111,6 +112,7 @@ catkin_package(
 #  LIBRARIES myworkcell_core
    CATKIN_DEPENDS roscpp fake_ar_publisher
    message_runtime geometry_msgs tf2_ros tf2_geometry_msgs
+   moveit_ros_planning_interface
 #  DEPENDS system_lib
 )
 

myworkcell_core/package.xml

@@ -53,6 +53,7 @@
   <build_depend>message_generation</build_depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
   <depend>geometry_msgs</depend>
   <depend>fake_ar_publisher</depend>
   <build_export_depend>roscpp</build_export_depend>

myworkcell_core/src/myworkcell_node.cpp

@@ -1,5 +1,9 @@
+#include <memory>
+
 #include <ros/ros.h>
+#include <tf/tf.h>
 #include <myworkcell_core/LocalizePart.h>
+#include <moveit/move_group_interface/move_group_interface.h>
 
 class ScanNPlan
 {
@@ -8,8 +12,15 @@ class ScanNPlan
   ros::ServiceClient vision_client_;
 
 public:
+
+  // Unique pointer to move group instance
+  std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group;
+
   ScanNPlan(ros::NodeHandle& nh)
   {
+    // Initialize moveit move group
+    move_group = std::make_unique<moveit::planning_interface::MoveGroupInterface>(moveit::planning_interface::MoveGroupInterface("manipulator"));
+
     vision_client_ = nh.serviceClient<myworkcell_core::LocalizePart>("localize_part");
   }
 
@@ -30,6 +41,30 @@ class ScanNPlan
     }
 
     ROS_INFO_STREAM("Part localized with " << srv.response);
+
+    geometry_msgs::Pose move_target = srv.response.pose;
+
+
+
+    // Plan for robot to move to localize part location
+    move_group->setPoseReferenceFrame(base_frame);
+
+    // Get current end-effector pose using default end-effector link
+    geometry_msgs::PoseStamped old_pose = move_group->getCurrentPose();
+
+    // Set target pose
+    move_group->setPoseTarget(move_target);
+
+    // Move to target pose
+    ROS_INFO_STREAM("Moving to target pose ...");
+    move_group->move();
+
+    // Move back to old pose after a small delay
+    ros::Duration(2).sleep();
+    ROS_INFO_STREAM("Moving back to home pose ... ");
+    move_group->setPoseTarget(old_pose);
+    move_group->move();
+
   }
 };
 
@@ -51,9 +86,14 @@ int main(int argc, char* argv[])
 
   ros::Duration(0.5).sleep();
 
+  // Adding asynchronous spinner for enabling move group planning
+  ros::AsyncSpinner async_spinner(1);
+  async_spinner.start();
+
   // Call ScanNPlan application start method
   // This start is called from the main thread of this Ros node
   app.start(base_frame_request);
 
-  ros::spin();
+  ros::waitForShutdown();
+
 }

myworkcell_moveit_config/rviz/moveit_rviz.rviz

@@ -3,10 +3,7 @@ Panels:
     Help Height: 0
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /Grid1
+      Expanded: ~
       Splitter Ratio: 0.5
     Tree Height: 140
   - Class: rviz/Selection
@@ -258,6 +255,14 @@ Visualization Manager:
         Show Robot Visual: true
       Value: true
       Velocity_Scaling_Factor: 1
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /ar_pose_visual
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255
@@ -309,14 +314,14 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 752
+  Height: 741
   Hide Left Dock: false
   Hide Right Dock: false
   MotionPlanning:
     collapsed: false
-  MotionPlanning - Slider:
+  MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f300000257fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000000c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000010c000001880000017d00ffffff000000010000010f00000257fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000257000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055600000039fc0100000002fb0000000800540069006d0065010000000000000556000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002480000025700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000024cfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000000c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000010c0000017d0000017d00fffffffb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100ffffff000000010000010f0000024cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000024c000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055600000039fc0100000002fb0000000800540069006d0065010000000000000556000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000035d0000024c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time: