35 #include <turtlebot_arm_block_manipulation/PickAndPlaceAction.h> 39 #include <geometry_msgs/PoseArray.h> 52 turtlebot_arm_block_manipulation::PickAndPlaceFeedback
feedback_;
53 turtlebot_arm_block_manipulation::PickAndPlaceResult
result_;
54 turtlebot_arm_block_manipulation::PickAndPlaceGoalConstPtr
goal_;
73 nh_(
"~"), as_(name, false), action_name_(name), arm_(
"arm"), gripper_(
"gripper")
76 nh_.
param(
"grasp_attach_time", attach_time, 0.8);
77 nh_.
param(
"grasp_detach_time", detach_time, 0.6);
85 target_pose_pub_ = nh_.
advertise<geometry_msgs::PoseStamped>(
"/target_pose", 1,
true);
90 ROS_INFO(
"[pick and place] Received goal!");
92 arm_link = goal_->frame;
93 gripper_open = goal_->gripper_open;
94 gripper_closed = goal_->gripper_closed;
106 if (goal_->topic.length() < 1)
114 ROS_INFO(
"[pick and place] Got goal from topic! %s", goal_->topic.c_str());
121 ROS_INFO(
"%s: Preempted", action_name_.c_str());
126 void pickAndPlace(
const geometry_msgs::Pose& start_pose,
const geometry_msgs::Pose& end_pose)
128 ROS_INFO(
"[pick and place] Picking. Also placing.");
130 geometry_msgs::Pose target;
138 target.position.z =
z_up;
143 target.position.z = start_pose.position.z;
153 target.position.z =
z_up;
159 target.position.z =
z_up;
164 target.position.z = end_pose.position.z;
174 target.position.z =
z_up;
189 ROS_DEBUG(
"[pick and place] Move arm to '%s' position", target.c_str());
192 ROS_ERROR(
"[pick and place] Set named target '%s' failed", target.c_str());
197 if (
bool(result) ==
true)
203 ROS_ERROR(
"[pick and place] Move to target failed (error %d)", result.val);
218 ROS_DEBUG(
"[pick and place] Move arm to [%.2f, %.2f, %.2f, %.2f]",
219 target.position.x, target.position.y, target.position.z,
tf::getYaw(target.orientation));
222 geometry_msgs::PoseStamped modiff_target;
223 modiff_target.header.frame_id =
arm_link;
224 modiff_target.pose = target;
226 double x = modiff_target.pose.position.x;
227 double y = modiff_target.pose.position.y;
228 double z = modiff_target.pose.position.z;
229 double d = sqrt(x*x + y*y);
233 ROS_ERROR(
"Target pose out of reach [%f > %f]", d, 0.3);
241 double rp =
M_PI_2 - std::asin((d - 0.1)/0.205) + attempts*
fRand(-0.05, +0.05);
242 double ry = std::atan2(y, x) + attempts*
fRand(-0.05, +0.05);
249 ROS_DEBUG(
"z increase: %f + %f", modiff_target.pose.position.z, std::abs(std::cos(rp))/50.0);
250 modiff_target.pose.position.z += std::abs(std::cos(rp))/50.0;
252 ROS_DEBUG(
"Set pose target [%.2f, %.2f, %.2f] [d: %.2f, r: %.2f, p: %.2f, y: %.2f]", x, y, z, d, rr, rp, ry);
253 target_pose_pub_.
publish(modiff_target);
257 ROS_ERROR(
"Set pose target [%.2f, %.2f, %.2f, %.2f] failed",
258 modiff_target.pose.position.x, modiff_target.pose.position.y, modiff_target.pose.position.z,
265 if (
bool(result) ==
true)
271 ROS_ERROR(
"[pick and place] Move to target failed (error %d) at attempt %d",
272 result.val, attempts + 1);
277 ROS_ERROR(
"[pick and place] Move to target failed after %d attempts", attempts);
289 ROS_DEBUG(
"[pick and place] Set gripper opening to %f", opening);
292 ROS_ERROR(
"[pick and place] Set gripper opening to %f failed", opening);
297 if (
bool(result) ==
true)
303 ROS_ERROR(
"[pick and place] Set gripper opening failed (error %d)", result.val);
311 return ((
float(rand()) /
float(RAND_MAX)) * (max - min)) + min;
318 int main(
int argc,
char** argv)
320 ros::init(argc, argv,
"pick_and_place_action_server");
float fRand(float min, float max)
boost::shared_ptr< const Goal > acceptNewGoal()
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
bool setGripper(float opening)
ros::Publisher target_pose_pub_
void publish(const boost::shared_ptr< M > &message) const
PickAndPlaceServer(const std::string name)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool setJointValueTarget(const std::vector< double > &group_variable_values)
turtlebot_arm_block_manipulation::PickAndPlaceResult result_
static double getYaw(const Quaternion &bt_q)
turtlebot_arm_block_manipulation::PickAndPlaceFeedback feedback_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server
void setPoseReferenceFrame(const std::string &pose_reference_frame)
actionlib::SimpleActionServer< turtlebot_arm_block_manipulation::PickAndPlaceAction > as_
bool setPoseTarget(const Eigen::Affine3d &end_effector_pose, const std::string &end_effector_link="")
moveit::planning_interface::MoveGroupInterface gripper_
void setGoalOrientationTolerance(double tolerance)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
void setGoalPositionTolerance(double tolerance)
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
int main(int argc, char **argv)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
bool moveArmTo(const std::string &target)
void registerPreemptCallback(boost::function< void()> cb)
bool moveArmTo(const geometry_msgs::Pose &target)
void pickAndPlace(const geometry_msgs::Pose &start_pose, const geometry_msgs::Pose &end_pose)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void spin(CallbackQueue *queue=0)
moveit::planning_interface::MoveGroupInterface arm_
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
void allowReplanning(bool flag)
void sendGoalFromTopic(const geometry_msgs::PoseArrayConstPtr &msg)
void registerGoalCallback(boost::function< void()> cb)
ros::Subscriber pick_and_place_sub_
bool setNamedTarget(const std::string &name)
turtlebot_arm_block_manipulation::PickAndPlaceGoalConstPtr goal_