pick_demo

.vscode/settings.json

@@ -81,7 +81,8 @@
         "typeinfo": "cpp",
         "valarray": "cpp",
         "variant": "cpp",
-        "bit": "cpp"
+        "bit": "cpp",
+        "future": "cpp"
     },
     "C_Cpp.errorSquiggles": "disabled"
 }

air_moveit_config/CMakeLists.txt

@@ -59,6 +59,7 @@ demo(alternative_path_costs)
 demo(ik_clearance_cost)
 demo(fallbacks_move_to)
 demo(detect_and_add_cylinder_collision_object_demo)
+demo(pick_demo)
 demo(pick_place_demo)
 demo(detect_and_add)
 demo(add_sphere)

air_moveit_config/include/pick_demo.hpp

@@ -0,0 +1,36 @@
+#ifndef PICK_DEMO_HPP
+#define PICK_DEMO_HPP
+
+#include <ros/ros.h>
+#include <moveit_msgs/PlanningScene.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+
+
+class air_pick
+{
+    public:
+
+    air_pick(ros::NodeHandle& nh);
+    ~air_pick(){delete move_group;}
+
+    void planningSceneWorldCallback(const moveit_msgs::PlanningScene::ConstPtr& msg);
+    void pick();
+    std_msgs::Float32MultiArray m_xyz;
+
+
+    ros::NodeHandle m_NodeHandle;
+    ros::Subscriber m_cord_sub;
+    const std::string PLANNING_GROUP_ARM = "air";
+    moveit::planning_interface::MoveGroupInterface* move_group;
+
+    float m_x;
+    float m_y;
+    float m_z;
+
+    private:
+
+};
+
+#endif // PICK_DEMO_HPP

air_moveit_config/launch/air_cloud.launch

@@ -12,7 +12,7 @@
   <!-- By default, we do not start a database (it can be large) -->
   <arg name="db" default="false" />
   <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find qb_hand_moveit_config)/default_warehouse_mongo_db" />
+  <arg name="db_path" default="$(find air_moveit_config)/default_warehouse_mongo_db" />
 
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />

air_moveit_config/launch/moveit.rviz

@@ -22,7 +22,7 @@ Panels:
   - Class: rviz/Time
     Name: Time
     SyncMode: 1
-    SyncSource: ""
+    SyncSource: PointCloud2
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -58,7 +58,7 @@ Visualization Manager:
       MoveIt_Allow_Sensor_Positioning: false
       MoveIt_Planning_Attempts: 10
       MoveIt_Planning_Time: 5
-      MoveIt_Use_Cartesian_Path: true
+      MoveIt_Use_Cartesian_Path: false
       MoveIt_Use_Constraint_Aware_IK: false
       MoveIt_Workspace:
         Center:
@@ -586,7 +586,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
@@ -601,7 +601,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

air_moveit_config/src/add_sphere.cpp

@@ -3,6 +3,7 @@
 #include <moveit_msgs/CollisionObject.h>
 #include <geometry_msgs/Vector3.h>
 #include <std_msgs/Float32MultiArray.h>
+#include <cmath>
 
 ros::Subscriber sub;
 
@@ -12,7 +13,7 @@ void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr xyz)
 { 
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
   // Get the sphere dimensions from the message
-  float radius = 0.05;
+  float radius = 0.04; // Radius of sphere
   float x = xyz->data[0];
   float y = xyz->data[1];
   float z = xyz->data[2];
@@ -25,9 +26,12 @@ void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr xyz)
 
   // Specify the pose of the sphere
   geometry_msgs::Pose pose;
-  pose.position.x = z; // x to y
-  pose.position.y = -x; // y to -z
-  pose.position.z = -y; //z to x 
+  // To fix data transformations -> x = z, y = -x, z = -y
+  float cam_degree = 30; // Angle between robot and camera
+  pose.position.x = z + (((radius/2) / sin(((90-cam_degree) * M_PI)/180))*2);
+  pose.position.y = -x; 
+  pose.position.z = -y + radius; //
+  pose.orientation.w = 1;
 
   moveit_msgs::CollisionObject collision_object;
   collision_object.header.frame_id = "camera_link";

air_moveit_config/src/pick_demo.cpp

@@ -0,0 +1,85 @@
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include "../include/pick_demo.hpp"
+
+air_pick::air_pick(ros::NodeHandle& nh)
+  : m_NodeHandle(nh) 
+{
+  move_group = new moveit::planning_interface::MoveGroupInterface(PLANNING_GROUP_ARM);
+  ros::Duration(4.0).sleep();
+  m_cord_sub = m_NodeHandle.subscribe<moveit_msgs::PlanningScene>("/move_group/monitored_planning_scene", 1000, &air_pick::planningSceneWorldCallback,this);
+
+  ROS_INFO("Initializing completed.");
+  }
+
+
+void air_pick::pick(){
+
+  move_group->setPlanningTime(45.0);
+  geometry_msgs::Pose pick_pose, place_pose;
+  geometry_msgs::PoseStamped current_pose;
+
+  ROS_INFO("Object Pose: x= %f, y= %f, z= %f ",m_x,m_y,m_z);
+
+  // Pick Pose
+  current_pose = move_group->getCurrentPose("tool0");
+  pick_pose.position.x = m_x;
+  pick_pose.position.y = m_y;
+  pick_pose.position.z = m_z + 0.05;
+  pick_pose.orientation = current_pose.pose.orientation;
+
+  // Place Pose
+  place_pose.position.x = m_x;
+  place_pose.position.y = m_y + 0,05;
+  place_pose.position.z = m_z + 0.1;
+  move_group->setPoseTarget(pick_pose);
+
+  // Plan and execute the pick motion
+  moveit::planning_interface::MoveGroupInterface::Plan plan;
+  moveit::planning_interface::MoveItErrorCode result = move_group->plan(plan);
+
+  if (result == moveit::planning_interface::MoveItErrorCode::SUCCESS)
+  {
+    move_group->execute(plan);
+  }
+  else
+  {
+    ROS_ERROR("Failed to plan pick motion");
+  }
+
+  ros::shutdown;
+}
+
+
+// Callback function for the subscriber
+void air_pick::planningSceneWorldCallback(const moveit_msgs::PlanningScene::ConstPtr& msg)
+{ 
+  ROS_INFO("CALLBACK");
+  // Get coordinates of object in world
+  if (msg->world.collision_objects.size() > 0) {
+      geometry_msgs::Point position = msg->world.collision_objects[0].pose.position;
+      m_x = position.x;
+      m_y = position.y;
+      m_z = position.z;
+      ROS_INFO("Received position: (%f, %f, %f)", position.x, position.y, position.z);
+  }
+  else{
+      ROS_INFO("No collision object detected in planning scene!");
+  }
+
+  // Kill collision updater node
+  //system("rosnode kill air_collision_publisher");
+
+  pick(); 
+}
+
+
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "pick_and_place");
+  ros::NodeHandle nh;
+  air_pick obj(nh);
+  ros::spinOnce();
+  return 0;
+}