void GoToSelectedBall::executeCB(const scheduler::SchedulerGoalConstPtr &goal){
ROS_INFO("enter executeCB, goal = %i", goal->value);
if(goal->value == 0){
state_ = STOP;
}
else if(goal->value == 1){
state_ = FIRST_STEP_COLLECT;
}
else if(goal->value == 2){
// TODO: sprawdza, czy jest ustawiona pozycja pileczki, albo przesylac ja razem z goalem
// TODO: dopisac serwer do jazdy do przodu a nie na sleep tak jak teraz
state_ = SECOND_STEP_COLLECT;
goForward(0);
ROS_INFO("enter SECOND_STEP_COLLECT");
publishAngle();
ac.waitForResult();
float dist = getDistanceFromSelectedBall();
onHoover();
goForward(dist -0.3);
ros::Duration(4.0).sleep();
goForward(-(dist-0.3));
ros::Duration(5.0).sleep();
goForward(0);
offHoover();
ROS_INFO("leave SECOND_STEP_COLLECT");
}
as_.publishFeedback(feedback_);
result_.value = feedback_.value;
as_.setSucceeded(result_);
ROS_INFO("leave executeCB");
}
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();
}
/*
bool ArmMotionInterface::plan_path_current_to_goal_flange_pose() {
ROS_INFO("computing a joint-space trajectory to right-arm flange goal pose");
//unpack the goal pose:
goal_flange_affine_ = xformUtils.transformPoseToEigenAffine3d(cart_goal_.des_pose_flange.pose);
ROS_INFO("flange goal");
display_affine(goal_flange_affine_);
Vectorq7x1 q_start;
q_start = get_jspace_start_(); // choose last cmd, or current joint angles
path_is_valid_ = cartTrajPlanner_.cartesian_path_planner(q_start, goal_flange_affine_, optimal_path_);
if (path_is_valid_) {
baxter_traj_streamer_.stuff_trajectory_right_arm(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message
computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
cart_result_.return_code = cartesian_planner::cart_moveResult::SUCCESS;
cart_result_.computed_arrival_time = computed_arrival_time_;
cart_move_as_.setSucceeded(cart_result_);
} else {
cart_result_.return_code = cartesian_planner::cart_moveResult::PATH_NOT_VALID;
cart_result_.computed_arrival_time = -1.0; //impossible arrival time
cart_move_as_.setSucceeded(cart_result_); //the communication was a success, but not the computation
}
return path_is_valid_;
}
*/
bool ArmMotionInterface::plan_jspace_path_current_to_cart_gripper_pose() {
ROS_INFO("computing a jspace trajectory to right-arm gripper goal pose");
//unpack the goal pose:
goal_gripper_pose_ = cart_goal_.des_pose_gripper;
xformUtils.printStampedPose(goal_gripper_pose_);
goal_flange_affine_ = xform_gripper_pose_to_affine_flange_wrt_torso(goal_gripper_pose_);
ROS_INFO("flange goal");
display_affine(goal_flange_affine_);
Vectorq7x1 q_start;
q_start = get_jspace_start_(); // choose last cmd, or current joint angles
// bool jspace_path_planner_to_affine_goal(Vectorq7x1 q_start, Eigen::Affine3d a_flange_end, std::vector<Eigen::VectorXd> &optimal_path);
path_is_valid_ = cartTrajPlanner_.jspace_path_planner_to_affine_goal(q_start, goal_flange_affine_, optimal_path_);
if (path_is_valid_) {
baxter_traj_streamer_.stuff_trajectory_right_arm(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message
computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
cart_result_.return_code = cartesian_planner::cart_moveResult::SUCCESS;
cart_result_.computed_arrival_time = computed_arrival_time_;
cart_move_as_.setSucceeded(cart_result_);
} else {
cart_result_.return_code = cartesian_planner::cart_moveResult::PATH_NOT_VALID;
cart_result_.computed_arrival_time = -1.0; //impossible arrival time
cart_move_as_.setSucceeded(cart_result_); //the communication was a success, but not the computation
}
return path_is_valid_;
}
//this is a pretty general function:
// goal contains a desired tool pose;
// path is planned from current joint state to some joint state that achieves desired tool pose
bool ArmMotionInterface::rt_arm_plan_path_current_to_goal_pose() {
ROS_INFO("computing a cartesian trajectory to right-arm tool goal pose");
//unpack the goal pose:
goal_gripper_pose_right_ = cart_goal_.des_pose_gripper_right;
goal_gripper_affine_right_= transformPoseToEigenAffine3d(goal_gripper_pose_right_.pose);
ROS_INFO("tool frame goal: ");
display_affine(goal_gripper_affine_right_);
goal_flange_affine_right_ = goal_gripper_affine_right_ * A_tool_wrt_flange_.inverse();
ROS_INFO("flange goal");
display_affine(goal_flange_affine_right_);
Vectorq7x1 q_start;
q_start = get_jspace_start_right_arm_(); // choose last cmd, or current joint angles
path_is_valid_ = cartTrajPlanner_.cartesian_path_planner(q_start, goal_flange_affine_right_, optimal_path_);
if (path_is_valid_) {
baxter_traj_streamer_.stuff_trajectory(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message
computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
cart_result_.computed_arrival_time = computed_arrival_time_;
cart_move_as_.setSucceeded(cart_result_);
}
else {
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
cart_result_.computed_arrival_time = -1.0; //impossible arrival time
cart_move_as_.setSucceeded(cart_result_); //the communication was a success, but not the computation
}
return path_is_valid_;
}
bool ArmMotionInterface::rt_arm_plan_path_current_to_goal_dp_xyz() {
Eigen::Vector3d dp_vec;
ROS_INFO("called rt_arm_plan_path_current_to_goal_dp_xyz");
//unpack the goal pose:
int ndim = cart_goal_.arm_dp_right.size();
if (ndim!=3) {
ROS_WARN("requested displacement, arm_dp_right, is wrong dimension");
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
path_is_valid_=false;
return path_is_valid_;
}
for (int i=0;i<3;i++) dp_vec[i] = cart_goal_.arm_dp_right[i];
ROS_INFO("requested dp = %f, %f, %f",dp_vec[0],dp_vec[1],dp_vec[2]);
Vectorq7x1 q_start;
q_start = get_jspace_start_right_arm_(); // choose last cmd, or current joint angles
path_is_valid_= plan_cartesian_delta_p(q_start, dp_vec);
if (path_is_valid_) {
baxter_traj_streamer_.stuff_trajectory(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message
computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
cart_result_.computed_arrival_time = computed_arrival_time_;
cart_move_as_.setSucceeded(cart_result_);
}
else {
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
cart_result_.computed_arrival_time = -1.0; //impossible arrival time
cart_move_as_.setSucceeded(cart_result_); //the communication was a success, but not the computation
}
return path_is_valid_;
}
void executeCB(const speedydug_servers::SpinBucketGoalConstPtr &goal)
{
// helper variables
bucket_detected_ = false;
ir_detected_ = false;
feedback_.success = false;
float z;
ros::Duration d(0.05);
if( goal->left ){
z = ANG_VEL;
}
else {
z = -1.0 * ANG_VEL;
}
ROS_INFO(goal->left ? "true" : "false");
ROS_INFO("z = %f", z);
while(!bucket_detected_ && !ir_detected_ )
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
twist_.angular.z = 0;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
break;
}
twist_.angular.z = z;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
as_.publishFeedback(feedback_);
d.sleep();
}
if(bucket_detected_ )
{
twist_.angular.z = 0;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
result_.success = true;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
} else if(ir_detected_) {
twist_.angular.z = 0;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
result_.success = false;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
}
void Navigator::executeCB(const actionlib::SimpleActionServer<navigator::navigatorAction>::GoalConstPtr& goal) {
int destination_id = goal->location_code;
geometry_msgs::PoseStamped destination_pose;
int navigation_status;
if (destination_id==navigator::navigatorGoal::COORDS) {
destination_pose=goal->desired_pose;
}
switch(destination_id) {
case navigator::navigatorGoal::HOME:
//specialized function to navigate to pre-defined HOME coords
navigation_status = navigate_home();
if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
ROS_INFO("reached home");
result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
navigator_as_.setSucceeded(result_);
}
else {
ROS_WARN("could not navigate home!");
navigator_as_.setAborted(result_);
}
break;
case navigator::navigatorGoal::TABLE:
//specialized function to navigate to pre-defined TABLE coords
navigation_status = navigate_to_table();
if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
ROS_INFO("reached table");
result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
navigator_as_.setSucceeded(result_);
}
else {
ROS_WARN("could not navigate to table!");
navigator_as_.setAborted(result_);
}
break;
case navigator::navigatorGoal::COORDS:
//more general function to navigate to specified pose:
destination_pose=goal->desired_pose;
navigation_status = navigate_to_pose(destination_pose);
if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
ROS_INFO("reached desired pose");
result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
navigator_as_.setSucceeded(result_);
}
else {
ROS_WARN("could not navigate to desired pose!");
navigator_as_.setAborted(result_);
}
break;
default:
ROS_WARN("this location ID is not implemented");
result_.return_code = navigator::navigatorResult::DESTINATION_CODE_UNRECOGNIZED;
navigator_as_.setAborted(result_);
}
}
//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long. Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <example_action_server::demoAction> customizes the simple action server to use our own "action" message
// defined in our package, "example_action_server", in the subdirectory "action", called "demo.action"
// The name "demo" is prepended to other message types created automatically during compilation.
// e.g., "demoAction" is auto-generated from (our) base name "demo" and generic name "Action"
void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<example_action_server::demoAction>::GoalConstPtr& goal) {
//ROS_INFO("in executeCB");
//ROS_INFO("goal input is: %d", goal->input);
//do work here: this is where your interesting code goes
//....
// for illustration, populate the "result" message with two numbers:
// the "input" is the message count, copied from goal->input (as sent by the client)
// the "goal_stamp" is the server's count of how many goals it has serviced so far
// if there is only one client, and if it is never restarted, then these two numbers SHOULD be identical...
// unless some communication got dropped, indicating an error
// send the result message back with the status of "success"
g_count++; // keep track of total number of goals serviced since this server was started
result_.output = g_count; // we'll use the member variable result_, defined in our class
result_.goal_stamp = goal->input;
// the class owns the action server, so we can use its member methods here
// DEBUG: if client and server remain in sync, all is well--else whine and complain and quit
// NOTE: this is NOT generically useful code; server should be happy to accept new clients at any time, and
// no client should need to know how many goals the server has serviced to date
if (g_count != goal->input) {
ROS_WARN("hey--mismatch!");
ROS_INFO("g_count = %d; goal_stamp = %d", g_count, result_.goal_stamp);
g_count_failure = true; //set a flag to commit suicide
ROS_WARN("informing client of aborted goal");
as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
}
else {
as_.setSucceeded(result_); // tell the client that we were successful acting on the request, and return the "result" message
}
}
//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long. Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <Action_Server::gazebo.action> customizes the simple action server to use our own "action" message
// defined in our package, "Action_Server", in the subdirectory "action", called "Action.action"
// The name "Action" is prepended to other message types created automatically during compilation.
// e.g., "gazebo.action" is auto-generated from (our) base name "Action" and generic name "Action"
void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<Action_Server::gazebo.action>::GoalConstPtr& goal) {
ROS_INFO("in executeCB");
ROS_INFO("goal input is: %d", goal->input);
//do work here: this is where your interesting code goes
ros::Rate timer(1.0); // 1Hz timer
countdown_val_ = goal->input;
//implement a simple timer, which counts down from provided countdown_val to 0, in seconds
while (countdown_val_>0) {
ROS_INFO("countdown = %d",countdown_val_);
// each iteration, check if cancellation has been ordered
if (as_.isPreemptRequested()){
ROS_WARN("goal cancelled!");
result_.output = countdown_val_;
as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
return; // done with callback
}
//if here, then goal is still valid; provide some feedback
feedback_.fdbk = countdown_val_; // populate feedback message with current countdown value
as_.publishFeedback(feedback_); // send feedback to the action client that requested this goal
countdown_val_--; //decrement the timer countdown
timer.sleep(); //wait 1 sec between loop iterations of this timer
}
//if we survive to here, then the goal was successfully accomplished; inform the client
result_.output = countdown_val_; //value should be zero, if completed countdown
as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
}
void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) {
ROS_INFO("in executeCB: received manipulation task");
ROS_INFO("object code is: %d", goal->object_code);
ROS_INFO("perception_source is: %d", goal->perception_source);
g_status_code = 0; //coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
g_action_code = 1; //start with perceptual processing
//do work here: this is where your interesting code goes
while ((g_status_code != SUCCESS)&&(g_status_code != ABORTED)) { //coordinator::ManipTaskResult::MANIP_SUCCESS) {
feedback_.feedback_status = g_status_code;
as_.publishFeedback(feedback_);
//ros::Duration(0.1).sleep();
ROS_INFO("executeCB: g_status_code = %d",g_status_code);
// each iteration, check if cancellation has been ordered
if (as_.isPreemptRequested()) {
ROS_WARN("goal cancelled!");
result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
g_action_code = 0;
g_status_code = 0;
as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
return; // done with callback
}
//here is where we step through states:
switch (g_action_code) {
case 1:
ROS_INFO("starting new task; should call object finder");
g_status_code = 1; //
ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
ros::Duration(2.0).sleep();
g_action_code = 2;
break;
case 2: // also do nothing...but maybe comment on status? set a watchdog?
g_status_code = 2;
ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
ros::Duration(2.0).sleep();
g_action_code = 3;
break;
case 3:
g_status_code = SUCCESS; //coordinator::ManipTaskResult::MANIP_SUCCESS; //code 0
ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
g_action_code = 0; // back to waiting state--regardless
break;
default:
ROS_WARN("executeCB: error--case not recognized");
break;
}
ros::Duration(0.5).sleep();
}
ROS_INFO("executeCB: manip success; returning success");
//if we survive to here, then the goal was successfully accomplished; inform the client
result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
g_action_code = 0;
g_status_code = 0;
return;
}
/*!
* \brief Executes the callback from the actionlib
*
* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
* \param goal JointTrajectoryGoal
*/
void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
{
ROS_INFO("sdh: executeCB");
if (!isInitialized_)
{
ROS_ERROR("%s: Rejected, sdh not initialized", action_name_.c_str());
as_.setAborted();
return;
}
while (hasNewGoal_ == true ) usleep(10000);
// \todo TODO: use joint_names for assigning values
targetAngles_.resize(DOF_);
targetAngles_[0] = goal->trajectory.points[0].positions[2]*180.0/pi_; // sdh_knuckle_joint
targetAngles_[1] = goal->trajectory.points[0].positions[5]*180.0/pi_; // sdh_finger22_joint
targetAngles_[2] = goal->trajectory.points[0].positions[6]*180.0/pi_; // sdh_finger23_joint
targetAngles_[3] = goal->trajectory.points[0].positions[0]*180.0/pi_; // sdh_thumb2_joint
targetAngles_[4] = goal->trajectory.points[0].positions[1]*180.0/pi_; // sdh_thumb3_joint
targetAngles_[5] = goal->trajectory.points[0].positions[3]*180.0/pi_; // sdh_finger12_joint
targetAngles_[6] = goal->trajectory.points[0].positions[4]*180.0/pi_; // sdh_finger13_joint
ROS_INFO("received new position goal: [['sdh_thumb_2_joint', 'sdh_thumb_3_joint', 'sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint']] = [%f,%f,%f,%f,%f,%f,%f,%f]",goal->trajectory.points[0].positions[0],goal->trajectory.points[0].positions[1],goal->trajectory.points[0].positions[2],goal->trajectory.points[0].positions[3],goal->trajectory.points[0].positions[4],goal->trajectory.points[0].positions[5],goal->trajectory.points[0].positions[6]);
hasNewGoal_ = true;
usleep(500000); // needed sleep until sdh starts to change status from idle to moving
bool finished = false;
while(finished == false)
{
if (as_.isNewGoalAvailable())
{
ROS_WARN("%s: Aborted", action_name_.c_str());
as_.setAborted();
return;
}
for ( size_t i = 0; i < state_.size(); i++ )
{
ROS_DEBUG("state[%d] = %d",i,state_[i]);
if (state_[i] == 0)
{
finished = true;
}
else
{
finished = false;
}
}
usleep(10000);
//feedback_ =
//as_.send feedback_
}
// set the action state to succeeded
ROS_INFO("%s: Succeeded", action_name_.c_str());
//result_.result.data = "succesfully received new goal";
//result_.success = 1;
//as_.setSucceeded(result_);
as_.setSucceeded();
}
void setJointsCB( const staubliTX60::SetJointsGoalConstPtr &goal ) {
//staubli.ResetMotion();
ros::Rate rate(10);
bool success = true;
ROS_INFO("Set Joints Action Cmd received \n");
if( staubli.MoveJoints(goal->j,
goal->params.movementType,
goal->params.jointVelocity,
goal->params.jointAcc,
goal->params.jointDec,
goal->params.endEffectorMaxTranslationVel,
goal->params.endEffectorMaxRotationalVel,
goal->params.distBlendPrev,
goal->params.distBlendNext
) )
{
ROS_INFO("Cmd received, moving to desired joint angles.");
while(true){
if (as_.isPreemptRequested() || !ros::ok()) {
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
staubli.ResetMotion();
as_.setPreempted();
success = false;
break;
}
if( polling(goal->j) ) break;
rate.sleep();
}
if(success) as_.setSucceeded(result_);
}else {
as_.setAborted();
ROS_ERROR("Cannot move to specified joints' configuration.");
}
}
void moveBlock(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
{
geometry_msgs::Pose start_pose_bumped, end_pose_bumped;
start_pose_bumped = start_pose;
start_pose_bumped.position.y -= bump_size;
//start_pose_bumped.position.z -= block_size/2.0 - bump_size;
start_pose_bumped.position.z = CUBE_Z_HEIGHT;
action_result_.pickup_pose = start_pose_bumped;
end_pose_bumped = end_pose;
//end_pose_bumped.position.z -= block_size/2.0 - bump_size;
end_pose_bumped.position.z = CUBE_Z_HEIGHT;
action_result_.place_pose = end_pose_bumped;
geometry_msgs::PoseArray msg;
msg.header.frame_id = arm_link;
msg.header.stamp = ros::Time::now();
msg.poses.push_back(start_pose_bumped);
msg.poses.push_back(end_pose_bumped);
pick_place_pub_.publish(msg);
action_server_.setSucceeded(action_result_);
// DTC server_.clear();
// DTC server_.applyChanges();
}
void ControlDoneCB(const actionlib::SimpleClientGoalState& state, const oea_controller::controlPlatformResultConstPtr &result)
{
controlling_ = false;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Control Action finished: " << state.toString());
move_result_.result_state = result->result_state;
move_result_.error_string = result->error_string;
if (move_result_.result_state)
{
as_.setSucceeded(move_result_);
ROS_INFO_NAMED(logger_name_, "Goal was successful :)");
}
else
{
ROS_WARN_NAMED(logger_name_, "Goal was NOT successful :)");
// if is preempted => as_ was already set, cannot set again
if (state.toString() != "PREEMPTED")
{
as_.setAborted(move_result_);
ROS_DEBUG_NAMED(logger_name_, "Goal was Aborted");
}
else
{
if (set_terminal_state_)
{
as_.setPreempted(move_result_);
ROS_DEBUG_NAMED(logger_name_, "Goal was Preempted");
}
}
}
}
//take in q_start and q_end and build trivial path in optimal_path_ for pure joint-space move
bool ArmMotionInterface::plan_jspace_path_qstart_to_qend(Vectorq7x1 q_start, Vectorq7x1 q_goal) {
ROS_INFO("setting up a joint-space path");
path_is_valid_ = cartTrajPlanner_.jspace_trivial_path_planner(q_start, q_goal, optimal_path_);
if (path_is_valid_) {
baxter_traj_streamer_.stuff_trajectory(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message
computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
cart_result_.computed_arrival_time = computed_arrival_time_;
cart_move_as_.setSucceeded(cart_result_);
}
else {
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
cart_result_.computed_arrival_time = -1.0; //impossible arrival time
cart_move_as_.setSucceeded(cart_result_); //the communication was a success, but not the computation
}
return path_is_valid_;
}
bool ArmMotionInterface::refine_cartesian_path_soln(void) {
ROS_INFO("ArmMotionInterface: refining trajectory");
if (path_is_valid_) {
bool valid = cartTrajPlanner_.refine_cartesian_path_plan(optimal_path_);
if (!valid) return false;
baxter_traj_streamer_.stuff_trajectory_right_arm(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message
computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
cart_result_.return_code = cartesian_planner::cart_moveResult::SUCCESS;
cart_result_.computed_arrival_time = computed_arrival_time_;
cart_move_as_.setSucceeded(cart_result_);
} else {
cart_result_.return_code = cartesian_planner::cart_moveResult::PATH_NOT_VALID;
cart_result_.computed_arrival_time = -1.0; //impossible arrival time
cart_move_as_.setSucceeded(cart_result_); //the communication was a success, but not the computation
return false;
}
return true;
}
void timer_callback(const ros::TimerEvent &) {
if (!c3trajectory) return;
ros::Time now = ros::Time::now();
if (actionserver.isPreemptRequested()) {
current_waypoint = c3trajectory->getCurrentPoint();
current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities
current_waypoint_t = now;
// don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
c3trajectory_t = now;
}
if (actionserver.isNewGoalAvailable()) {
boost::shared_ptr<const uf_common::MoveToGoal> goal = actionserver.acceptNewGoal();
current_waypoint = subjugator::C3Trajectory::Waypoint(
Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist), goal->speed,
!goal->uncoordinated);
current_waypoint_t = now; // goal->header.stamp;
this->linear_tolerance = goal->linear_tolerance;
this->angular_tolerance = goal->angular_tolerance;
c3trajectory_t = now;
}
while ((c3trajectory_t + traj_dt < now) and ros::ok()) {
// ROS_INFO("Acting");
c3trajectory->update(traj_dt.toSec(), current_waypoint,
(c3trajectory_t - current_waypoint_t).toSec());
c3trajectory_t += traj_dt;
}
PoseTwistStamped msg;
msg.header.stamp = c3trajectory_t;
msg.header.frame_id = fixed_frame;
msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
trajectory_pub.publish(msg);
PoseStamped posemsg;
posemsg.header.stamp = c3trajectory_t;
posemsg.header.frame_id = fixed_frame;
posemsg.pose = Pose_from_Waypoint(current_waypoint);
waypoint_pose_pub.publish(posemsg);
if (actionserver.isActive() &&
c3trajectory->getCurrentPoint().is_approximately(
current_waypoint.r, max(1e-3, linear_tolerance), max(1e-3, angular_tolerance)) &&
current_waypoint.r.qdot == subjugator::Vector6d::Zero()) {
actionserver.setSucceeded();
}
}
/***********************************Callback**************************************************/
void goalCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
std::vector<std::string> jointNames = goal->trajectory.joint_names;
if (checkIfValid(jointNames)) {
size_t commandSize = goal->trajectory.points.size();
for (int i = 0; i < commandSize - 1; ++i) {
sensor_msgs::JointState command;
command.name = goal->trajectory.joint_names;
command.position = goal->trajectory.points[i].positions;
command.velocity = goal->trajectory.points[i].velocities;
command.effort = goal->trajectory.points[i].effort;
_jointCommand.publish(command);
ROS_INFO_STREAM(goal->trajectory.points[i]);
clrFeedback();
_feedback.joint_names = jointNames;
_feedback.desired.effort = goal->trajectory.points[i].effort;
_feedback.desired.velocities = goal->trajectory.points[i].velocities;
_feedback.desired.positions = goal->trajectory.points[i].positions;
waitForExecution();
}
sensor_msgs::JointState command;
command.name = goal->trajectory.joint_names;
command.position = goal->trajectory.points[commandSize - 1].positions;
for(int i = 0; i < command.name.size(); ++i)
{
command.velocity.push_back(0.2);
}
command.effort = goal->trajectory.points[commandSize - 1].effort;
rosInfo("Last");
_jointCommand.publish(command);
clrFeedback();
_feedback.joint_names = jointNames;
_feedback.desired.effort = goal->trajectory.points[commandSize - 1].effort;
_feedback.desired.velocities = goal->trajectory.points[commandSize - 1].velocities;
_feedback.desired.positions = goal->trajectory.points[commandSize - 1].positions;
waitForExecution();
_result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
_result.error_string = "Cool";
_actionServer.setSucceeded(_result);
}
else {
_result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
_result.error_string = "Not cool";
_actionServer.setAborted(_result);
}
}
void as_cb(const cob_sound::SayGoalConstPtr &goal)
{
bool ret = say(goal->text.data);
if (ret)
{
as_.setSucceeded();
}
else
{
as_.setAborted();
}
}
/*!
* \brief Executes a valid trajectory.
*
* Move the arm to a goal configuration in Joint Space
* \param goal JointTrajectoryGoalConstPtr
*/
void executeTrajectory(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
{
ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
if(!executing_)
{
while(current_operation_mode_ != "velocity")
{
ROS_INFO("waiting for arm to go to velocity mode");
usleep(100000);
}
traj_ = goal->trajectory;
if(traj_.points.size() == 1)
{
traj_generator_->moveThetas(traj_.points[0].positions, q_current);
}
else
{
//Insert the current point as first point of trajectory, then generate SPLINE trajectory
trajectory_msgs::JointTrajectoryPoint p;
p.positions.resize(7);
p.velocities.resize(7);
p.accelerations.resize(7);
for(unsigned int i = 0; i<7; i++)
{
p.positions.at(i) = q_current.at(i);
p.velocities.at(i) = 0.0;
p.accelerations.at(i) = 0.0;
}
std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it;
it = traj_.points.begin();
traj_.points.insert(it,p);
traj_generator_->moveTrajectory(traj_, q_current);
}
executing_ = true;
startposition_ = q_current;
//TODO: std::cout << "Trajectory time: " << traj_end_time_ << " Trajectory step: " << traj_step_ << "\n";
}
else //suspend current movement and start new one
{
}
while(executing_)
{
sleep(1);
}
as_.setSucceeded();
}
void ArmMotionInterface::execute_planned_move(void) {
if (!path_is_valid_) {
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
ROS_WARN("attempted to execute invalid path!");
cart_move_as_.setAborted(cart_result_); // tell the client we have given up on this goal; send the result message as well
}
// convert path to a trajectory:
//baxter_traj_streamer_.stuff_trajectory(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message
js_goal_.trajectory = des_trajectory_;
//computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
ROS_INFO("sending action request to traj streamer node");
ROS_INFO("computed arrival time is %f", computed_arrival_time_);
busy_working_on_a_request_ = true;
traj_streamer_action_client_.sendGoal(js_goal_, boost::bind(&ArmMotionInterface::js_doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired
finished_before_timeout_ = traj_streamer_action_client_.waitForResult(ros::Duration(computed_arrival_time_ + 2.0));
if (!finished_before_timeout_) {
ROS_WARN("EXECUTE_PLANNED_PATH: giving up waiting on result");
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT;
cart_move_as_.setSucceeded(cart_result_); //could say "aborted"
} else {
ROS_INFO("finished before timeout");
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
cart_move_as_.setSucceeded(cart_result_);
}
path_is_valid_ = false; // reset--require new path before next move
busy_working_on_a_request_ = false;
//save the last point commanded, for future reference
std::vector <double> last_pt;
last_pt = des_trajectory_.points.back().positions;
int njnts = last_pt.size();
for (int i=0;i<njnts;i++) {
last_arm_jnt_cmd_right_[i] = last_pt[i];
}
}
/*!
* \brief Executes the callback from the actionlib.
*
* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
* \param goal JointTrajectoryGoal
*/
void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
{
ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
if (!isInitialized_)
{
ROS_ERROR("%s: Rejected, powercubes not initialized", action_name_.c_str());
as_.setAborted();
return;
}
// saving goal into local variables
traj_ = goal->trajectory;
traj_point_nr_ = 0;
traj_point_ = traj_.points[traj_point_nr_];
finished_ = false;
// stoping arm to prepare for new trajectory
std::vector<double> VelZero;
VelZero.resize(ModIds_param_.size());
PCube_->MoveVel(VelZero);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
}
usleep(500000); // needed sleep until powercubes starts to change status from idle to moving
while(finished_ == false)
{
if (as_.isNewGoalAvailable())
{
ROS_WARN("%s: Aborted", action_name_.c_str());
as_.setAborted();
return;
}
usleep(10000);
//feedback_ =
//as_.send feedback_
}
// set the action state to succeed
//result_.result.data = "executing trajectory";
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
//void executeCB()
void executeCB(const amazon_challenge_bt_actions::BTGoalConstPtr &goal)
{
// helper variables
ros::Rate r(1);
bool success = true;
// publish info to the console for the user
ROS_INFO("Starting Action");
// start executing the action
for(int i=1; i<=20; i++)
{
ROS_INFO("Executing Action");
//motion_proxy_ptr->post.move(0.1, 0.0, 0.0);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("Action Halted");
// motion_proxy_ptr->stopMove();
// set the action state to preempted
as_.setPreempted();
success = false;
break;
}
feedback_.status = RUNNING;
// publish the feedback
as_.publishFeedback(feedback_);
// this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep();
}
if(success)
{
result_.status = feedback_.status;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
}
//void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) {
void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
if(isInitialized_) {
ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
// saving goal into local variables
traj_ = goal->trajectory;
traj_point_nr_ = 0;
traj_point_ = traj_.points[traj_point_nr_];
GoalPos_ = traj_point_.positions[0];
finished_ = false;
// stoping axis to prepare for new trajectory
CamAxis_->Stop();
// check that preempt has not been requested by the client
if (as_.isPreemptRequested())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
}
usleep(2000000); // needed sleep until drive starts to change status from idle to moving
while (not finished_)
{
if (as_.isNewGoalAvailable())
{
ROS_WARN("%s: Aborted", action_name_.c_str());
as_.setAborted();
return;
}
usleep(10000);
//feedback_ =
//as_.send feedback_
}
// set the action state to succeed
//result_.result.data = "executing trajectory";
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
} else {
as_.setAborted();
ROS_WARN("Camera_axis not initialized yet!");
}
}
void executeFollowTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal)
{
ROS_INFO("Received new goal trajectory with %lu points",goal->trajectory.points.size());
spawnTrajector(goal->trajectory);
// only set to succeeded if component could reach position. this is currently not the care for e.g. by emergency stop, hardware error or exceeds limit.
if(rejected_)
as_follow_.setAborted(); //setRejected not implemented in simpleactionserver ?
else
{
if(failure_)
as_follow_.setAborted();
else
as_follow_.setSucceeded();
}
rejected_ = false;
failure_ = false;
}
void action_execute_stop_callback(const s8_motor_controller::StopGoalConstPtr & goal) {
ROS_INFO("STOP");
if(is_stopping) {
ROS_FATAL("Stop action callback executed but is already stopping");
}
stop();
is_stopping = true;
const int timeout = 10; // 10 seconds.
const int rate_hz = 10;
ros::Rate rate(rate_hz);
const int encoder_callback_ticks_treshold = 10;
int ticks = 0;
while(!is_still && ticks <= timeout * rate_hz && ticks_since_last_encoder_callback < encoder_callback_ticks_treshold) {
rate.sleep();
ticks++;
ticks_since_last_encoder_callback++;
}
if(ticks_since_last_encoder_callback >= encoder_callback_ticks_treshold) {
ROS_INFO("No encoder callback in %d ticks. Assuming still.", ticks_since_last_encoder_callback);
is_still = true;
}
if(!is_still) {
ROS_WARN("Unable to stop. Stop action failed.");
s8_motor_controller::StopResult stop_action_result;
stop_action_result.stopped = false;
stop_action.setAborted(stop_action_result);
} else {
s8_motor_controller::StopResult stop_action_result;
stop_action_result.stopped = true;
stop_action.setSucceeded(stop_action_result);
ROS_INFO("Stop action succeeded");
}
is_stopping = false;
ignore_twist = true;
ignored_twist_cnt = 0;
}
//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long. Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <example_action_server::demoAction> customizes the simple action server to use our own "action" message
// defined in our package, "example_action_server", in the subdirectory "action", called "demo.action"
// The name "demo" is prepended to other message types created automatically during compilation.
// e.g., "demoAction" is auto-generated from (our) base name "demo" and generic name "Action"
void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<example_action_server::demoAction>::GoalConstPtr& goal) {
ROS_INFO("callback activated");
double yaw_desired, yaw_current, travel_distance, spin_angle;
nav_msgs::Path paths;
geometry_msgs::Pose pose_desired;
paths = goal->input;
int npts = paths.poses.size();
ROS_INFO("received path request with %d poses",npts);
int move = 0;
for (int i=0;i<npts;i++) { //visit each subgoal
// odd notation: drill down, access vector element, drill some more to get pose
pose_desired = paths.poses[i].pose; //get first pose from vector of poses
get_yaw_and_dist(g_current_pose, pose_desired,travel_distance, yaw_desired);
ROS_INFO("pose %d: desired yaw = %f; desired (x,y) = (%f,%f)",i,yaw_desired,
pose_desired.position.x,pose_desired.position.y);
ROS_INFO("current (x,y) = (%f, %f)",g_current_pose.position.x,g_current_pose.position.y);
ROS_INFO("travel distance = %f",travel_distance);
ROS_INFO("pose %d: desired yaw = %f",i,yaw_desired);
yaw_current = convertPlanarQuat2Phi(g_current_pose.orientation); //our current yaw--should use a sensor
spin_angle = yaw_desired - yaw_current; // spin this much
//spin_angle = min_spin(spin_angle);// but what if this angle is > pi? then go the other way
do_spin(spin_angle); // carry out this incremental action
// we will just assume that this action was successful--really should have sensor feedback here
g_current_pose.orientation = pose_desired.orientation; // assumes got to desired orientation precisely
move = pose_desired.position.x;
ROS_INFO("Move: %d", move);
do_move(move);
if(as_.isPreemptRequested()){
do_halt();
ROS_WARN("Srv: goal canceled.");
result_.output = -1;
as_.setAborted(result_);
return;
}
}
result_.output = 0;
as_.setSucceeded(result_);
ROS_INFO("Move finished.");
}
void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) {
ROS_INFO("in executeCB: received manipulation task");
ROS_INFO("object code is: %d", goal->object_code);
ROS_INFO("perception_source is: %d", goal->perception_source);
//do work here: this is where your interesting code goes
feedback_.feedback_status = coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
as_.publishFeedback(feedback_);
ros::Duration(1.0).sleep(); // pretend we are processing
//get pickup pose:
feedback_.feedback_status = coordinator::ManipTaskFeedback::PERCEPTION_BUSY;
as_.publishFeedback(feedback_);
ros::Duration(1.0).sleep(); // pretend we are processing
feedback_.feedback_status = coordinator::ManipTaskFeedback::PICKUP_PLANNING_BUSY;
as_.publishFeedback(feedback_);
ros::Duration(1.0).sleep(); // pretend we are processing
feedback_.feedback_status = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY;
as_.publishFeedback(feedback_);
ros::Duration(1.0).sleep(); // pretend we are processing
feedback_.feedback_status = coordinator::ManipTaskFeedback::DROPOFF_PLANNING_BUSY;
as_.publishFeedback(feedback_);
ros::Duration(1.0).sleep(); // pretend we are processing
feedback_.feedback_status = coordinator::ManipTaskFeedback::DROPOFF_MOTION_BUSY;
as_.publishFeedback(feedback_);
ros::Duration(1.0).sleep(); // pretend we are processing
// each iteration, check if cancellation has been ordered
if (as_.isPreemptRequested()) {
ROS_WARN("goal cancelled!");
result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
return; // done with callback
}
//if we survive to here, then the goal was successfully accomplished; inform the client
result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
}
void executeCB(const learning_actionlib::FibonacciGoalConstPtr &goal)
{
// helper variables
ros::Rate r(1);
bool success = true;
// push_back the seeds for the fibonacci sequence
feedback_.sequence.clear();
feedback_.sequence.push_back(0);
feedback_.sequence.push_back(1);
// publish info to the console for the user
ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
// start executing the action
for(int i=1; i<=goal->order; i++)
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
break;
}
feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
// publish the feedback
as_.publishFeedback(feedback_);
// this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep();
}
if(success)
{
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
}
void executeCB(const autopnp_scenario::AnalyzeMapGoalConstPtr &goal)
{
ros::Rate r(1);
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(goal->input_img, sensor_msgs::image_encodings::MONO8);
cv::Mat original_img;
original_img = cv_ptr->image;
cv::Mat Segmented_map ;
Segmented_map = exploration_obj.Image_Segmentation( original_img , goal->map_resolution );
r.sleep();
//Publish Result message:
cv_bridge::CvImage cv_image;
cv_image.header.stamp = ros::Time::now();
cv_image.encoding = "mono8";
cv_image.image = Segmented_map;
cv_image.toImageMsg(result_.output_img);
result_.room_center_x = exploration_obj.get_Center_of_Room_x();
result_.room_center_y = exploration_obj.get_Center_of_Room_y();
result_.map_resolution = goal->map_resolution;
result_.Map_Origin_x = goal->Map_Origin_x;
result_.Map_Origin_y = goal->Map_Origin_y;
result_.room_min_x = exploration_obj.get_room_min_x();
result_.room_min_y = exploration_obj.get_room_min_y();
result_.room_max_x = exploration_obj.get_room_max_x();
result_.room_max_y = exploration_obj.get_room_max_y();
exploration_obj.get_Center_of_Room_x().clear();
exploration_obj.get_Center_of_Room_y().clear();
exploration_obj.get_room_min_x().clear();
exploration_obj.get_room_max_x().clear();
exploration_obj.get_room_min_y().clear();
exploration_obj.get_room_max_y().clear();
ams_.setSucceeded(result_);
}
