-
Notifications
You must be signed in to change notification settings - Fork 5
/
pick_and_placer.cpp
62 lines (53 loc) · 1.76 KB
/
pick_and_placer.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
// Copyright 2017 Geoffrey Biggs (geoffrey.biggs@aist.go.jp)
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/GripperCommandAction.h>
int main(int argc, char **argv) {
ros::init(argc, argv, "pickandplacer");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(2);
spinner.start();
// Set up the arm planning interface
moveit::planning_interface::MoveGroupInterface arm("arm");
// Specify end-effector positions in the "base_link" task frame
arm.setPoseReferenceFrame("base_link");
// Create a client to command the gripper
actionlib::SimpleActionClient<control_msgs::GripperCommandAction> gripper(
"/crane_plus_gripper/gripper_command",
"true");
gripper.waitForServer();
// Prepare
ROS_INFO("Moving to prepare pose");
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "base_link";
pose.pose.position.x = 0.2;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.1;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.707106;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 0.707106;
// Plan a move to the pose
arm.setPoseTarget(pose);
// Execute the move
if (!arm.move()) {
ROS_WARN("Could not move to prepare pose");
return 1;
}
// Open gripper
ROS_INFO("Opening gripper");
control_msgs::GripperCommandGoal goal;
// Open the gripper to 10 cm wide
goal.command.position = 0.1;
// Send the gripper command
gripper.sendGoal(goal);
// Wait for the command to complete
bool finishedBeforeTimeout = gripper.waitForResult(ros::Duration(30));
if (!finishedBeforeTimeout) {
ROS_WARN("Gripper open action did not complete");
return 1;
}
ros::shutdown();
return 0;
}