如何通过 ROS Moveit 操作 UR5？

问题描述

我想操纵我的机械臂 (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 函数，我的 UR5e 成功移动到 CAMERA_JOINT 位置，但不再继续

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

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）

c++moveit ros ros