void processGoal(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose )
{
// Change the goal constraints on the servos to be less strict, so that the controllers don't die. this is a hack
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/elbow_pitch_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/shoulder_pan_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/gripper_roll_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45
if( !pickAndPlace(start_block_pose, end_block_pose) )
{
ROS_ERROR_STREAM_NAMED("pick_place_moveit","Pick and place failed");
if(action_server_.isActive()) // Make sure we haven't sent a fake goal
{
// Report failure
result_.success = false;
action_server_.setSucceeded(result_);
}
}
else
{
if(action_server_.isActive()) // Make sure we haven't sent a fake goal
{
// Report success
result_.success = true;
action_server_.setSucceeded(result_);
}
}
// TODO: remove
ros::shutdown();
}
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;
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();
}
