diff --git a/README.md b/README.md
index ef41e32..31c86cd 100644
--- a/README.md
+++ b/README.md
@@ -36,5 +36,7 @@ This repository contains practice code of [ros industrial training melodic branc
```
-
+
- 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.
diff --git a/myworkcell_core/CMakeLists.txt b/myworkcell_core/CMakeLists.txt
index 66a6761..c7c13da 100644
--- a/myworkcell_core/CMakeLists.txt
+++ b/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
)
diff --git a/myworkcell_core/package.xml b/myworkcell_core/package.xml
index 96b10e1..8b1fa39 100644
--- a/myworkcell_core/package.xml
+++ b/myworkcell_core/package.xml
@@ -53,6 +53,7 @@
message_generation
tf2_ros
tf2_geometry_msgs
+ moveit_ros_planning_interface
geometry_msgs
fake_ar_publisher
roscpp
diff --git a/myworkcell_core/src/myworkcell_node.cpp b/myworkcell_core/src/myworkcell_node.cpp
index 73e38e9..5733e81 100644
--- a/myworkcell_core/src/myworkcell_node.cpp
+++ b/myworkcell_core/src/myworkcell_node.cpp
@@ -1,5 +1,9 @@
+#include
+
#include
+#include
#include
+#include
class ScanNPlan
{
@@ -8,8 +12,15 @@ class ScanNPlan
ros::ServiceClient vision_client_;
public:
+
+ // Unique pointer to move group instance
+ std::unique_ptr move_group;
+
ScanNPlan(ros::NodeHandle& nh)
{
+ // Initialize moveit move group
+ move_group = std::make_unique(moveit::planning_interface::MoveGroupInterface("manipulator"));
+
vision_client_ = nh.serviceClient("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();
+
}
diff --git a/myworkcell_moveit_config/rviz/moveit_rviz.rviz b/myworkcell_moveit_config/rviz/moveit_rviz.rviz
index 8789133..a4365ec 100644
--- a/myworkcell_moveit_config/rviz/moveit_rviz.rviz
+++ b/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: