void ServerGoalHandleDestructionTester::goalCallback(GoalHandle gh) { ROS_ERROR_NAMED("actionlib", "In callback"); //assign to our stored goal handle *gh_ = gh; TestGoal goal = *gh.getGoal(); switch (goal.goal) { case 1: gh.setAccepted(); gh.setSucceeded(TestResult(), "The ref server has succeeded"); break; case 2: gh.setAccepted(); gh.setAborted(TestResult(), "The ref server has aborted"); break; case 3: gh.setRejected(TestResult(), "The ref server has rejected"); break; default: break; } ros::shutdown(); }
void goalCB(GoalHandle gh) { std::string nodeName = ros::this_node::getName(); ROS_INFO("%s",(nodeName + ": Received grasping goal").c_str()); switch(gh.getGoal()->goal) { case GraspHandPostureExecutionGoal::PRE_GRASP: gh.setAccepted(); //gh.getGoal()->grasp ROS_INFO("%s",(nodeName + ": Pre-grasp command accepted").c_str()); gh.setSucceeded(); break; case GraspHandPostureExecutionGoal::GRASP: gh.setAccepted(); ROS_INFO("%s",(nodeName + ": Executing a gripper grasp").c_str()); // wait ros::Duration(0.25f).sleep(); gh.setSucceeded(); break; case GraspHandPostureExecutionGoal::RELEASE: gh.setAccepted(); ROS_INFO("%s",(nodeName + ": Executing a gripper release").c_str()); // wait ros::Duration(0.25f).sleep(); gh.setSucceeded(); break; default: ROS_INFO("%s",(nodeName + ": Unidentified grasp request, rejecting goal").c_str()); gh.setSucceeded(); //gh.setRejected(); break; } }
void goalCB(GoalHandle gh) { // Ensures that the joints in the goal match the joints we are commanding. ROS_DEBUG("Received goal: goalCB"); if (!setsEqual(joint_names_, gh.getGoal()->trajectory.joint_names)) { ROS_ERROR("Joints on incoming goal don't match our joints"); gh.setRejected(); return; } // Cancels the currently active goal. if (has_active_goal_) { ROS_DEBUG("Received new goal, canceling current goal"); // Stops the controller. trajectory_msgs::JointTrajectory empty; empty.joint_names = joint_names_; pub_controller_command_.publish(empty); // Marks the current goal as canceled. active_goal_.setCanceled(); has_active_goal_ = false; } // Sends the trajectory along to the controller if (withinGoalConstraints(last_controller_state_, goal_constraints_, gh.getGoal()->trajectory)) { ROS_INFO_STREAM("Already within goal constraints"); gh.setAccepted(); gh.setSucceeded(); } else { gh.setAccepted(); active_goal_ = gh; has_active_goal_ = true; ROS_INFO("Publishing trajectory"); current_traj_ = active_goal_.getGoal()->trajectory; pub_controller_command_.publish(current_traj_); } }
void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperFindContactDataConstPtr &msg) { last_controller_state_ = msg; ros::Time now = ros::Time::now(); if (!has_active_goal_) return; pr2_gripper_sensor_msgs::PR2GripperFindContactGoal goal; goal.command = active_goal_.getGoal()->command; pr2_gripper_sensor_msgs::PR2GripperFindContactFeedback feedback; feedback.data = *msg; pr2_gripper_sensor_msgs::PR2GripperFindContactResult result; result.data = *msg; // do not check until some dT has expired from message start double dT = 0.2; if(feedback.data.stamp.toSec() < action_start_time.toSec()+dT ){} // if we are actually in a find_contact control state or positoin control state else if(feedback.data.rtstate.realtime_controller_state == 5 || feedback.data.rtstate.realtime_controller_state == 3) { if(feedback.data.contact_conditions_met) { active_goal_.setSucceeded(result); has_active_goal_ = false; } } else has_active_goal_ = false; active_goal_.publishFeedback(feedback); }
void goalCB(GoalHandle gh) { GripperMessage gMsg; SimpleMessage request; SimpleMessage reply; ROS_DEBUG("Received grasping goal"); while (!(robot_->isConnected())) { ROS_DEBUG("Reconnecting"); robot_->makeConnect(); } switch(gh.getGoal()->goal) { case GraspHandPostureExecutionGoal::PRE_GRASP: gh.setAccepted(); ROS_WARN("Pre-grasp is not supported by this gripper"); gh.setSucceeded(); break; case GraspHandPostureExecutionGoal::GRASP: case GraspHandPostureExecutionGoal::RELEASE: gh.setAccepted(); switch(gh.getGoal()->goal) { case GraspHandPostureExecutionGoal::GRASP: ROS_INFO("Executing a gripper grasp"); gMsg.init(GripperOperationTypes::CLOSE); break; case GraspHandPostureExecutionGoal::RELEASE: ROS_INFO("Executing a gripper release"); gMsg.init(GripperOperationTypes::OPEN); break; } gMsg.toRequest(request); this->robot_->sendAndReceiveMsg(request, reply); switch(reply.getReplyCode()) { case ReplyTypes::SUCCESS: ROS_INFO("Robot gripper returned success"); gh.setSucceeded(); break; case ReplyTypes::FAILURE: ROS_ERROR("Robot gripper returned failure"); gh.setCanceled(); break; } break; default: gh.setRejected(); break; } }
void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg) { //ROS_DEBUG("Checking controller state feedback"); last_controller_state_ = msg; ros::Time now = ros::Time::now(); if (!has_active_goal_) { //ROS_DEBUG("No active goal, ignoring feedback"); return; } if (current_traj_.points.empty()) { ROS_DEBUG("Current trajectory is empty, ignoring feedback"); return; } /* NOT CONCERNED ABOUT TRAJECTORY TIMING AT THIS POINT if (now < current_traj_.header.stamp + current_traj_.points[0].time_from_start) return; */ if (!setsEqual(joint_names_, msg->joint_names)) { ROS_ERROR("Joint names from the controller don't match our joint names."); return; } // Checking for goal constraints // Checks that we have ended inside the goal constraints and has motion stopped ROS_DEBUG("Checking goal constraints"); if (withinGoalConstraints(msg, goal_constraints_, current_traj_)) { // Additional check for motion stoppage since the controller goal may still // be moving. The current robot driver calls a motion stop if it receives // a new trajectory while it is still moving. If the driver is not publishing // the motion state (i.e. old driver), this will still work, but it warns you. if (last_robot_status_.in_motion.val == industrial_msgs::TriState::FALSE) { ROS_INFO("Inside goal constraints, stopped moving, return success for action"); active_goal_.setSucceeded(); has_active_goal_ = false; } else if (last_robot_status_.in_motion.val == industrial_msgs::TriState::UNKNOWN) { ROS_INFO("Inside goal constraints, return success for action"); ROS_WARN("Robot status: in motion unknown, the robot driver node and controller code should be updated"); active_goal_.setSucceeded(); has_active_goal_ = false; } else { ROS_DEBUG("Within goal constraints but robot is still moving"); } } // Verifies that the controller has stayed within the trajectory constraints. /* DISABLING THIS MORE COMPLICATED GOAL CHECKING AND ERROR DETECTION int last = current_traj_.points.size() - 1; ros::Time end_time = current_traj_.header.stamp + current_traj_.points[last].time_from_start; if (now < end_time) { // Checks that the controller is inside the trajectory constraints. for (size_t i = 0; i < msg->joint_names.size(); ++i) { double abs_error = fabs(msg->error.positions[i]); double constraint = trajectory_constraints_[msg->joint_names[i]]; if (constraint >= 0 && abs_error > constraint) { // Stops the controller. trajectory_msgs::JointTrajectory empty; empty.joint_names = joint_names_; pub_controller_command_.publish(empty); active_goal_.setAborted(); has_active_goal_ = false; ROS_WARN("Aborting because we would up outside the trajectory constraints"); return; } } } else { // Checks that we have ended inside the goal constraints bool inside_goal_constraints = true; for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i) { double abs_error = fabs(msg->error.positions[i]); double goal_constraint = goal_constraints_[msg->joint_names[i]]; if (goal_constraint >= 0 && abs_error > goal_constraint) inside_goal_constraints = false; // It's important to be stopped if that's desired. if (fabs(msg->desired.velocities[i]) < 1e-6) { if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_) inside_goal_constraints = false; } } if (inside_goal_constraints) { active_goal_.setSucceeded(); has_active_goal_ = false; } else if (now < end_time + ros::Duration(goal_time_constraint_)) { // Still have some time left to make it. } else { ROS_WARN("Aborting because we wound up outside the goal constraints"); active_goal_.setAborted(); has_active_goal_ = false; } } */ }
void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg) { //ROS_DEBUG("Checking controller state feedback"); last_controller_state_ = msg; ros::Time now = ros::Time::now(); if (!has_active_goal_) { //ROS_DEBUG("No active goal, ignoring feedback"); return; } if (current_traj_.points.empty()) { ROS_DEBUG("Current trajecotry is empty, ignoring feedback"); return; } /* NOT CONCERNED ABOUT TRAJECTORY TIMING AT THIS POINT if (now < current_traj_.header.stamp + current_traj_.points[0].time_from_start) return; */ if (!setsEqual(joint_names_, msg->joint_names)) { ROS_ERROR("Joint names from the controller don't match our joint names."); return; } // Checking for goal constraints // Checks that we have ended inside the goal constraints ROS_DEBUG("Checking goal contraints"); if(withinGoalConstraints(msg, goal_constraints_, current_traj_)) { ROS_INFO("Inside goal contraints, return success for action"); active_goal_.setSucceeded(); has_active_goal_ = false; } // Verifies that the controller has stayed within the trajectory constraints. /* DISABLING THIS MORE COMPLICATED GOAL CHECKING AND ERROR DETECTION int last = current_traj_.points.size() - 1; ros::Time end_time = current_traj_.header.stamp + current_traj_.points[last].time_from_start; if (now < end_time) { // Checks that the controller is inside the trajectory constraints. for (size_t i = 0; i < msg->joint_names.size(); ++i) { double abs_error = fabs(msg->error.positions[i]); double constraint = trajectory_constraints_[msg->joint_names[i]]; if (constraint >= 0 && abs_error > constraint) { // Stops the controller. trajectory_msgs::JointTrajectory empty; empty.joint_names = joint_names_; pub_controller_command_.publish(empty); active_goal_.setAborted(); has_active_goal_ = false; ROS_WARN("Aborting because we would up outside the trajectory constraints"); return; } } } else { // Checks that we have ended inside the goal constraints bool inside_goal_constraints = true; for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i) { double abs_error = fabs(msg->error.positions[i]); double goal_constraint = goal_constraints_[msg->joint_names[i]]; if (goal_constraint >= 0 && abs_error > goal_constraint) inside_goal_constraints = false; // It's important to be stopped if that's desired. if (fabs(msg->desired.velocities[i]) < 1e-6) { if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_) inside_goal_constraints = false; } } if (inside_goal_constraints) { active_goal_.setSucceeded(); has_active_goal_ = false; } else if (now < end_time + ros::Duration(goal_time_constraint_)) { // Still have some time left to make it. } else { ROS_WARN("Aborting because we wound up outside the goal constraints"); active_goal_.setAborted(); has_active_goal_ = false; } } */ }
void goalCB(GoalHandle gh){ if (has_active_goal_) { // Stops the controller. if(creato){ pthread_cancel(trajectoryExecutor); creato=0; } pub_topic.publish(empty); // Marks the current goal as canceled. active_goal_.setCanceled(); has_active_goal_ = false; } gh.setAccepted(); active_goal_ = gh; has_active_goal_ = true; toExecute = gh.getGoal()->trajectory; //moveit::planning_interface::MoveGroup group_quad("quad_group"); ROS_INFO_STREAM("Trajectory received, processing"); int n_points = toExecute.points.size(); double meters = 0.0; double vel_cte = 0.5; double t = 0.0; double dt = 0.0; double traj [n_points][6]; //Get Position of Waypoints for (int k=0; k<toExecute.points.size(); k++) { geometry_msgs::Transform_<std::allocator<void> > punto=toExecute.points[k].transforms[0]; traj[k][0] = punto.translation.x; traj[k][1] = punto.translation.y; traj[k][2] = punto.translation.z; } //Determine distance between consecutive Waypoints //to get aproximate path lenght for (int k=0; k<toExecute.points.size()-1; k++) { traj[k][3] = traj[k+1][0] - traj[k][0]; traj[k][4] = traj[k+1][1] - traj[k][1]; traj[k][5] = traj[k+1][2] - traj[k][2]; meters += sqrt(traj[k][3]*traj[k][3]+traj[k][4]*traj[k][4]+traj[k][5]*traj[k][5]); } ROS_INFO_STREAM("Aprox distance: "<< meters << " meters"); //Determine time in cte velocity profile t = meters/vel_cte; //We suppose equal separation between waypoint, so we //can get the time we have between each waypoint dt = t/(toExecute.points.size()-1); //Determine the velocity between waypoints double max_vel = 0.3; double min_vel = -0.3; for (int k=0; k<toExecute.points.size()-1; k++) { //X-Velocity traj[k][3] /= dt; if (traj[k][3]>max_vel) { traj[k][3] = max_vel; } else if (traj[k][3]<min_vel) { traj[k][3] = min_vel; } //Y-Velocity traj[k][4] /= dt; if (traj[k][4]>max_vel) { traj[k][4] = max_vel; } else if (traj[k][4]<min_vel) { traj[k][4] = min_vel; } //Z-Velocity traj[k][5] /= dt; if (traj[k][5]>max_vel) { traj[k][5] = max_vel; } else if (traj[k][5]<min_vel) { traj[k][5] = min_vel; } } //Post process in narrow edges std::vector<double> edges; for(unsigned i=1; i<n_points-1;i++) { //Get the angles between two consecutives velocity vectors //Source:http://de.mathworks.com/matlabcentral/answers/16243-angle-between-two-vectors-in-3d double cross_i = traj[i][4]*traj[i-1][5] - traj[i][5]*traj[i-1][4]; double cross_j = (-1)*(traj[i][3]*traj[i-1][5] - traj[i][5]*traj[i-1][3]); double cross_k = traj[i][3]*traj[i-1][4] - traj[i][4]*traj[i-1][3]; double dot = traj[i][3]*traj[i-1][3] + traj[i][4]*traj[i-1][4] + traj[i][5]*traj[i-1][5]; double norm_cross = sqrt(cross_i*cross_i + cross_j*cross_j + cross_k*cross_k); double angle = atan2(norm_cross,dot)*180/M_PI; //ROS_INFO_STREAM(angle); if (angle >= 25) { edges.push_back(i); } } for(int i=0; i<edges.size(); i++) { ROS_INFO_STREAM(edges[i]); } std::string dir = path + "/trajectory_ompl.csv"; //Open file to write trajectory std::ofstream myFile; myFile.open(dir.c_str()); //Save all the values separated by comas except the last for(unsigned i=0; i<n_points-2;i++) { myFile << traj[i][0] <<"," << traj[i][1] <<"," << traj[i][2] <<"," /* << 0.0 <<"," << 0.0 <<"," << 0.0 <<",\n"; */ << traj[i][3] <<"," << traj[i][4] <<"," << traj[i][5] <<",\n"; } //Which is saved here, but without a coma in the last element. int i = n_points-1; myFile << traj[i][0] <<"," << traj[i][1] <<"," << traj[i][2] <<"," << 0.0 <<"," << 0.0 <<"," << 0.0 <<",\n"; myFile.close(); /* //Show trajectory for (int k=0; k<toExecute.points.size(); k++) { ROS_INFO_STREAM(traj[k][0]<<" "<<traj[k][1] <<" "<<traj[k][2] <<" "<< traj[k][3]<<" "<<traj[k][4] <<" "<<traj[k][5]); } */ //std::string command = "rosrun quad_gazebo trajectory_client ompl"; //system(command.c_str()); active_goal_.setSucceeded(); has_active_goal_=false; creato=0; ROS_INFO_STREAM("Calling the client to execute the trajectory, please wait..."); std::string command = "rosrun quad_gazebo trajectory_client ompl"; system(command.c_str()); }
void controllerStateCB(const pr2_controllers_msgs::JointControllerStateConstPtr &msg) { last_controller_state_ = msg; ros::Time now = ros::Time::now(); if (!has_active_goal_) return; // grab the goal threshold off the param server if(!node_.getParam("position_servo_position_tolerance", goal_threshold_)) ROS_ERROR("No position_servo_position_tolerance given in namespace: '%s')", node_.getNamespace().c_str()); // Ensures that the controller is tracking my setpoint. if (fabs(msg->set_point - active_goal_.getGoal()->command.position) > stall_velocity_threshold_) { if (now - goal_received_ < ros::Duration(1.0)) { return; } else { ROS_ERROR("Cancelling goal: Controller is trying to achieve a different setpoint."); active_goal_.setCanceled(); has_active_goal_ = false; } } pr2_controllers_msgs::Pr2GripperCommandFeedback feedback; feedback.position = msg->process_value; feedback.effort = msg->command; feedback.reached_goal = false; feedback.stalled = false; pr2_controllers_msgs::Pr2GripperCommandResult result; result.position = msg->process_value; result.effort = msg->command; result.reached_goal = false; result.stalled = false; // check if we've reached the goal if (fabs(msg->process_value - active_goal_.getGoal()->command.position) < goal_threshold_) { feedback.reached_goal = true; result.reached_goal = true; active_goal_.setSucceeded(result); has_active_goal_ = false; } else { // Determines if the gripper has stalled. if (fabs(msg->process_value_dot) > stall_velocity_threshold_) { last_movement_time_ = ros::Time::now(); } else if ((ros::Time::now() - last_movement_time_).toSec() > stall_timeout_ && active_goal_.getGoal()->command.max_effort != 0.0) { feedback.stalled = true; result.stalled = true; active_goal_.setAborted(result); has_active_goal_ = false; } } active_goal_.publishFeedback(feedback); }