From 5a69addc882bd840184b3941df87cb91a0acd04b Mon Sep 17 00:00:00 2001 From: mrceki <105711013+mrceki@users.noreply.github.com> Date: Tue, 21 Feb 2023 15:59:23 +0300 Subject: [PATCH] pick_demo --- .vscode/settings.json | 3 +- air_moveit_config/CMakeLists.txt | 1 + air_moveit_config/include/pick_demo.hpp | 36 ++++++++++ air_moveit_config/launch/air_cloud.launch | 2 +- air_moveit_config/launch/moveit.rviz | 8 +-- air_moveit_config/src/add_sphere.cpp | 12 ++-- air_moveit_config/src/pick_demo.cpp | 85 +++++++++++++++++++++++ 7 files changed, 137 insertions(+), 10 deletions(-) create mode 100644 air_moveit_config/include/pick_demo.hpp create mode 100644 air_moveit_config/src/pick_demo.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index af10b76..cbc39b8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -81,7 +81,8 @@ "typeinfo": "cpp", "valarray": "cpp", "variant": "cpp", - "bit": "cpp" + "bit": "cpp", + "future": "cpp" }, "C_Cpp.errorSquiggles": "disabled" } \ No newline at end of file diff --git a/air_moveit_config/CMakeLists.txt b/air_moveit_config/CMakeLists.txt index 2a9e853..614b120 100644 --- a/air_moveit_config/CMakeLists.txt +++ b/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) diff --git a/air_moveit_config/include/pick_demo.hpp b/air_moveit_config/include/pick_demo.hpp new file mode 100644 index 0000000..9bab18e --- /dev/null +++ b/air_moveit_config/include/pick_demo.hpp @@ -0,0 +1,36 @@ +#ifndef PICK_DEMO_HPP +#define PICK_DEMO_HPP + +#include +#include +#include +#include + + + +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 \ No newline at end of file diff --git a/air_moveit_config/launch/air_cloud.launch b/air_moveit_config/launch/air_cloud.launch index 8ef9c01..24d6bd7 100644 --- a/air_moveit_config/launch/air_cloud.launch +++ b/air_moveit_config/launch/air_cloud.launch @@ -12,7 +12,7 @@ - + diff --git a/air_moveit_config/launch/moveit.rviz b/air_moveit_config/launch/moveit.rviz index e3c4f1a..a9a65d5 100644 --- a/air_moveit_config/launch/moveit.rviz +++ b/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 diff --git a/air_moveit_config/src/add_sphere.cpp b/air_moveit_config/src/add_sphere.cpp index 8a74ec1..5e4080c 100644 --- a/air_moveit_config/src/add_sphere.cpp +++ b/air_moveit_config/src/add_sphere.cpp @@ -3,6 +3,7 @@ #include #include #include +#include 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"; diff --git a/air_moveit_config/src/pick_demo.cpp b/air_moveit_config/src/pick_demo.cpp new file mode 100644 index 0000000..dd8b82a --- /dev/null +++ b/air_moveit_config/src/pick_demo.cpp @@ -0,0 +1,85 @@ + +#include +#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("/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; +} \ No newline at end of file