I want to manipulate my robot arm (UR5e) to perform pick & place task
so I use ROS and Moveit on Ubuntu 18.04 and I refer to Moveit tutorials (link below)
And this is part of my code
const std::vector<double> CAMERA_JOINT = {-1.57899937, -1.63659524, -1.02328654, -2.0525072, 1.57446152, 0.0};
const std::vector<double> HOME_JOINT = {-3.14159265, -1.01839962, -1.43169359, -2.26212124, 1.57376339, 0.0};
class MyRobotPlanning
{
private:
const std::string PLANNING_GROUP = "manipulator";
const robot_state::JointModelGroup* joint_model_group;
moveit::planning_interface::MoveGroupInterface move_group;
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
public:
MyRobotPlanning(): move_group(PLANNING_GROUP)
{
move_group.allowReplanning(true);
move_group.setNumPlanningAttempts(10);
}
void goToGoalPose(geometry_msgs::Pose &pose);
void goToJointState(std::vector<double> setting_values);
};
bool robotMove(capston::FeedBack::Request &req,
capston::FeedBack::Response &res)
{
MyRobotPlanning UR;
cout << "ready to sort your tools!!" << endl;
if (req.next_action == 1)
{
UR.goToJointState(CAMERA_JOINT);
res.feed_back = true;
return true;
}
else if(req.next_action == 2)
{
ros::NodeHandle nh;
ros::ServiceClient client = nh.serviceClient<capston::GraspPose>("grasppose");
capston::GraspPose g_pose;
if (client.call(g_pose))
{
geometry_msgs::Pose goal_pose;
goal_pose.position.x = g_pose.response.grasp_pose.position.x;
goal_pose.position.y = g_pose.response.grasp_pose.position.y;
goal_pose.position.z = g_pose.response.grasp_pose.position.z;
goal_pose.orientation.w = 1.0;
UR.goToGoalPose(goal_pose);
ROS_INFO("Tool Number: %d", (int)g_pose.response.tool_number);
ROS_INFO("tool_pose: [%f, %f, %f]", g_pose.response.grasp_pose.position.x,
g_pose.response.grasp_pose.position.y,
g_pose.response.grasp_pose.position.z);
return true;
}
else
{
ROS_ERROR("Failed to call service");
return false;
}
}
cout << "This code is not complete";
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "action_part");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
ros::ServiceServer server = nh.advertiseService("feedback", robotMove);
ROS_INFO("Ready srv server!!!");
ros::spin();
return 0;
}
void MyRobotPlanning::goToGoalPose(geometry_msgs::Pose &pose)
{
move_group.setPoseTarget(pose);
ros::Duration(0.5).sleep();
move_group.move();
}
void MyRobotPlanning::goToJointState(std::vector<double> setting_values)
{
move_group.setJointValueTarget(setting_values);
ros::Duration(0.5).sleep();
move_group.move();
}
This is both server and client node
so it receives xyz coordinates of the object (that we want to grasp) from another node
and also receive next_action
from the other node and then move my UR5e
When it receives the service feedback.request.next_action = 1
so it calls MyRobotPlanning::goToJointState
function and my UR5e moves to CAMERA_JOINT
position successfully but it doesn't progress any more
I think there's something stuck in move_group.move()
but I don't know why