1

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)

https://github.com/ros-planning/moveit_tutorials/blob/melodic-devel/doc/move_group_interface/src/move_group_interface_tutorial.cpp

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

genpfault
  • 51,148
  • 11
  • 85
  • 139
GGU
  • 11
  • 1

0 Answers0