turtlebot_arm_block_manipulation block_manipulation_moveit (Phantom X Pincher)

asked 2022-08-16 11:33:30 -0500

atef008 gravatar image

updated 2022-08-27 08:59:40 -0500

Hi,

I am testing the turtlebot package for my robot phantom x pincher. Using the config file, I could move the robot with Motion Planning on Rviz. Furthermore, I want to make the robot move to the desired position (with coordinate), so I found this package turtlebot_arm_block_manipulation, but I need a 3d Camera to test it. So the question is, how can I test it without a camera? In other words, how can I just put the input position manually and make the robot move to this position?

I found the pick_and_place_action_server.cpp file, which takes command from the camera. I am a beginner in ROS. I want to know how I can manually put the goal coordinates.

Below is the code from turtlebot_arm_block_manipulation package:

// license removed for brevity    
#include <ros/ros.h>
#include <tf/tf.h>

#include <actionlib/server/simple_action_server.h>
#include <turtlebot_arm_block_manipulation/PickAndPlaceAction.h>

#include <moveit/move_group_interface/move_group_interface.h>

#include <geometry_msgs/PoseArray.h>

namespace turtlebot_arm_block_manipulation
{

class PickAndPlaceServer
{
private:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<turtlebot_arm_block_manipulation::PickAndPlaceAction> as_;
  std::string action_name_;

  turtlebot_arm_block_manipulation::PickAndPlaceFeedback     feedback_;
  turtlebot_arm_block_manipulation::PickAndPlaceResult       result_;
  turtlebot_arm_block_manipulation::PickAndPlaceGoalConstPtr goal_;

  ros::Publisher target_pose_pub_;
  ros::Subscriber pick_and_place_sub_;

  // Move groups to control arm and gripper with MoveIt!
  moveit::planning_interface::MoveGroupInterface arm_;
  moveit::planning_interface::MoveGroupInterface gripper_;

  // Pick and place parameters
  std::string arm_link;
  double gripper_open;
  double gripper_closed;
  double attach_time;
  double detach_time;
  double z_up;

public:
  PickAndPlaceServer(const std::string name) :
    nh_("~"), as_(name, false), action_name_(name), arm_("arm"), gripper_("gripper")
  {
    // Read specific pick and place parameters
    nh_.param("grasp_attach_time", attach_time, 0.8);
    nh_.param("grasp_detach_time", detach_time, 0.6);

    // Register the goal and feedback callbacks
    as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));

    as_.start();

    target_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/target_pose", 1, true);
  }

  void goalCB()
  {
    ROS_INFO("[pick and place] Received goal!");
    goal_ = as_.acceptNewGoal();
    arm_link = goal_->frame;
    gripper_open = goal_->gripper_open;
    gripper_closed = goal_->gripper_closed;
    z_up = goal_->z_up;

    arm_.setPoseReferenceFrame(arm_link);

    // Allow some leeway in position (meters) and orientation (radians)
    arm_.setGoalPositionTolerance(0.001);
    arm_.setGoalOrientationTolerance(0.1);

    // Allow replanning to increase the odds of a solution
    arm_.allowReplanning(true);

    if (goal_->topic.length() < 1)
      pickAndPlace(goal_->pickup_pose, goal_->place_pose);
    else
      pick_and_place_sub_ = nh_.subscribe(goal_->topic, 1, &PickAndPlaceServer::sendGoalFromTopic, this);
  }

  void sendGoalFromTopic(const geometry_msgs::PoseArrayConstPtr& msg)
  {
    ROS_INFO("[pick and place] Got goal from topic! %s", goal_->topic.c_str());
    pickAndPlace(msg->poses[0], msg->poses[1]);
    pick_and_place_sub_.shutdown();
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }

  void pickAndPlace(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
  {
    ROS_INFO("[pick and place] Picking. Also placing.");

    geometry_msgs::Pose target;

    /* open gripper */
    if (setGripper(gripper_open) == false)
      return;

    /* hover over */
    target = start_pose;
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* go down */
    target.position.z = start_pose.position.z;
    if (moveArmTo(target) == false)
      return;

    /* close gripper */
    if (setGripper(gripper_closed) == false)
      return;
    ros::Duration(attach_time).sleep(); // ensure that gripper properly grasp the cube before lifting the arm

    /* go up */
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* hover over */
    target = end_pose;
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* go down */
    target.position.z = end_pose.position.z;
    if (moveArmTo(target) == false ...
(more)
edit retag flag offensive close merge delete

Comments

Just call moveArmTo with the target pose.

ravijoshi gravatar image ravijoshi  ( 2022-08-18 05:24:23 -0500 )edit

@Ravi Joschi thank your for replying. Can you please explain more how can i do it ? should i just call moveArmTo(Target) ? and how should be the target ?

atef008 gravatar image atef008  ( 2022-08-18 06:16:32 -0500 )edit

May I request you to remove the copyright and format the code properly first? If this code is available online, please also share the reference. It is really hard to see the code as it is too long to read and not well formatted.

ravijoshi gravatar image ravijoshi  ( 2022-08-18 21:29:06 -0500 )edit

Thank you @Ravi Joshi for replying. Yeah the code is available online. This is the link https://github.com/turtlebot/turtlebo...

I will try also to format the code properly. i am just a beginner and i really don't know what should i do? i did the moveit robot configuration and i need now to write a code to pich and place objects. I want to write the input (The goal) manually then the robot will move to the desired positon and pick the object. Thank you a lot for helping me :)

atef008 gravatar image atef008  ( 2022-08-26 17:53:02 -0500 )edit

The code looks straightforward. I think it is a good exercise to review the source code you provided. Not only will it help you to understand the ROS action server, but also it will eventually lead to finding the answer to your question. Please give it a try. Anyway, I do not see any image-related info on the code you have shared. Please see pick_and_place_action_server.cpp#L190-L196. This is where the actual magic happens. It uses the moveit package to set the target pose using arm_.setNamedTarget(target) and then commands the arm to move using arm_.move(). You need to configure moveit before calling these functions. This is why I suggested going through the source code.

ravijoshi gravatar image ravijoshi  ( 2022-08-27 09:13:29 -0500 )edit