moveit_servoing arm stops while tracking

asked 2023-02-23 19:34:11 -0500

akumar3.1428 gravatar image

updated 2023-02-23 19:37:49 -0500

Hi, I am using pose_tracking_example.cpp to move my real arm and track the marker. In case when the marker is in a stationary position and I run the program, the robot reaches the same position in z, after that if I move my marker the arm does not move at all, In another case when the arm comes near to drone and I try to move the maker while the robot is in motion it tracks it in the z-direction. I am not sure why this is happening. Any help would be much appreciated

CODE

   // Get the current EE transform
  geometry_msgs::TransformStamped current_ee_tf;
  tracker.getCommandFrameTransform(current_ee_tf);
  std::cout << current_ee_tf << std::endl;

  // Convert it to a Pose
  geometry_msgs::PoseStamped target_pose;
  target_pose.header.frame_id = current_ee_tf.header.frame_id;
  target_pose.pose.position.x = current_ee_tf.transform.translation.x;
  target_pose.pose.position.y = current_ee_tf.transform.translation.y;
  target_pose.pose.position.z = current_ee_tf.transform.translation.z;
  target_pose.pose.orientation = current_ee_tf.transform.rotation;

  // Modify it a little bit
  target_pose.pose.position.x += 0.01;

  // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple
  // waypoints
  tracker.resetTargetPose();

  // Publish target pose
  target_pose.header.stamp = ros::Time::now();
  target_pose_pub.publish(target_pose);
  // Run the pose tracking in a new thread
  std::thread move_to_pose_thread(
      [&tracker, &lin_tol, &rot_tol] { tracker.moveToPose(lin_tol, rot_tol, 0.1 /* target pose timeout */); });

  ros::Rate loop_rate(50);
  while(ros::ok())
  {  std_msgs::Float64MultiArray val = listener.aruco_data;

  if (!val.data.empty()) {

    // Modify the pose target a little bit each cycle
    // This is a dynamic pose target
    // target_pose.pose.position.x = current_ee_tf.transform.translation.x;
    std::cout << "Current position is " << target_pose.pose.position.x << std::endl;
    target_pose.pose.position.z = val.data.at(2);
    std::cout << "Tareget position in z is " << val.data.at(2) << std::endl;

    target_pose.header.stamp = ros::Time::now();
    target_pose_pub.publish(target_pose);
    std::cout << "Message has been published" << std::endl;
    loop_rate.sleep();
  }
  else{
    std::cout << "data is not received" << std::endl;
    loop_rate.sleep();
  }}

  // Make sure the tracker is stopped and clean up
  tracker.stopMotion();
  move_to_pose_thread.join();

  return EXIT_SUCCESS;
}
edit retag flag offensive close merge delete