1

我想操纵我的机械臂（UR5e）来执行拾取和放置任务
，所以我在 Ubuntu 18.04 上使用 ROS 和 Moveit，我参考了 Moveit 教程（下面的链接）

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

这是我的代码的一部分

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();
}

这既是服务器节点又是客户端节点
，因此它从另一个节点接收对象（我们想要掌握）的 xyz 坐标，并从另一个节点
接收next_action，然后移动我的 UR5e

当它收到服务feedback.request.next_action = 1所以它调用MyRobotPlanning::goToJointState函数并且我的 UR5eCAMERA_JOINT成功移动到位置但它不再进步

我认为有东西卡住了，move_group.move()但我不知道为什么

4

0 回答 0