void goalCB(GoalHandle gh)
{
// Cancels the currently active goal.
if (has_active_goal_)
{
// Marks the current goal as canceled.
active_goal_.setCanceled();
has_active_goal_ = false;
}
gh.setAccepted();
active_goal_ = gh;
has_active_goal_ = true;
goal_received_ = ros::Time::now();
// update our zero values for 0.25 seconds
if(active_goal_.getGoal()->command.zero_fingertip_sensors)
{
std_srvs::Empty::Request req;
std_srvs::Empty::Response resp;
if(ros::service::exists("zero_fingertip_sensors",true))
{
ROS_INFO("updating zeros!");
ros::service::call("zero_fingertip_sensors",req,resp);
}
}
// Sends the command along to the controller.
pub_controller_command_.publish(active_goal_.getGoal()->command);
last_movement_time_ = ros::Time::now();
action_start_time = ros::Time::now();
}
void goalCB(GoalHandle gh)
{
// Ensures that the joints in the goal match the joints we are commanding.
ROS_DEBUG("Received goal: goalCB");
ROS_INFO("Received goal: goalCB");
if (!is_joint_set_ok(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;
}
gh.setAccepted();
active_goal_ = gh;
has_active_goal_ = true;
// Sends the trajectory along to the controller
ROS_INFO("Publishing trajectory; sending commands to the joint controller ");
current_traj_ = active_goal_.getGoal()->trajectory;
pub_controller_command_.publish(current_traj_);
}
void BaseTrajectoryActionController::goalCB(GoalHandle gh)
{
ROS_DEBUG("goalCB");
std::vector<std::string> joint_names(joints_.size());
for (size_t j = 0; j < joints_.size(); ++j)
joint_names[j] = joints_[j]->name;
// Ensures that the joints in the goal match the joints we are commanding.
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 (active_goal_)
{
// Marks the current goal as canceled.
active_goal_->setCanceled();
}
starting(); // reset trajectory
gh.setAccepted();
// Sends the trajectory along to the controller
active_goal_ = boost::shared_ptr<GoalHandle>(new GoalHandle(gh));
commandTrajectory(share_member(gh.getGoal(),gh.getGoal()->trajectory), active_goal_);
}
void watchdog(const ros::TimerEvent &e)
{
ros::Time now = ros::Time::now();
// Aborts the active goal if the controller does not appear to be active.
if (has_active_goal_)
{
bool should_abort = false;
if (!last_controller_state_)
{
should_abort = true;
ROS_WARN("Aborting goal because we have never heard a controller state message.");
}
else if ((now - last_controller_state_->stamp) > ros::Duration(5.0))
{
should_abort = true;
ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
(now - last_controller_state_->stamp).toSec());
}
if (should_abort)
{
// Marks the current goal as aborted.
active_goal_.setAborted();
has_active_goal_ = false;
}
}
}
void CartesianTrajectoryAction::goalCB(GoalHandle gh) {
// cancel active goal
if (active_goal_.isValid() && (active_goal_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)) {
active_goal_.setCanceled();
}
Goal g = gh.getGoal();
CartesianTrajectory* trj_ptr = new CartesianTrajectory;
*trj_ptr = g->trajectory;
CartesianTrajectoryConstPtr trj_cptr = CartesianTrajectoryConstPtr(trj_ptr);
port_cartesian_trajectory_command_.write(trj_cptr);
gh.setAccepted();
active_goal_ = gh;
}
void cancelCB(GoalHandle gh)
{
if (active_goal_ == gh)
{
/*
// Stops the controller.
if (last_controller_state_)
{
pr2_controllers_msgs::Pr2GripperCommand stop;
stop.position = last_controller_state_->process_value;
stop.max_effort = 0.0;
pub_controller_command_.publish(stop);
}
*/
// stop the real-time motor control
std_srvs::Empty::Request req;
std_srvs::Empty::Response resp;
if(ros::service::exists("stop_motor_output",true))
{
ROS_INFO("Stopping Motor Output");
ros::service::call("stop_motor_output",req,resp);
}
// Marks the current goal as canceled.
active_goal_.setCanceled();
has_active_goal_ = false;
}
}
void cancelCB(GoalHandle gh)
{
std::string nodeName = ros::this_node::getName();
ROS_INFO("%s",(nodeName + ": Canceling current grasp action").c_str());
//gh.setAccepted();
gh.setCanceled();
ROS_INFO("%s",(nodeName + ": Current grasp action has been canceled").c_str());
}
void goalCB(GoalHandle gh)
{
// Cancels the currently active goal.
if (has_active_goal_)
{
// Marks the current goal as canceled.
active_goal_.setCanceled();
has_active_goal_ = false;
}
gh.setAccepted();
active_goal_ = gh;
has_active_goal_ = true;
goal_received_ = ros::Time::now();
min_error_seen_ = 1e10;
// Sends the command along to the controller.
pub_controller_command_.publish(active_goal_.getGoal()->command);
last_movement_time_ = ros::Time::now();
}
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)
{
ROS_DEBUG("GoalHandle request received");
// accept new goals
gh.setAccepted();
// get goal from handle
const tf2_web_republisher::TFSubscriptionGoal::ConstPtr& goal = gh.getGoal();
// generate goal_info struct
boost::shared_ptr<ClientGoalInfo> goal_info = boost::make_shared<ClientGoalInfo>();
goal_info->handle = gh;
goal_info->client_ID_ = client_ID_count_++;
goal_info->timer_ = nh_.createTimer(ros::Duration(1.0 / goal->rate),
boost::bind(&TFRepublisher::processGoal, this, goal_info, _1));
std::size_t request_size_ = goal->source_frames.size();
goal_info->tf_subscriptions_.resize(request_size_);
for (std::size_t i=0; i<request_size_; ++i )
{
TFPair& tf_pair = goal_info->tf_subscriptions_[i];
std::string source_frame = cleanTfFrame(goal->source_frames[i]);
std::string target_frame = cleanTfFrame(goal->target_frame);
tf_pair.setSourceFrame(source_frame);
tf_pair.setTargetFrame(target_frame);
tf_pair.setAngularThres(goal->angular_thres);
tf_pair.setTransThres(goal->trans_thres);
}
{
boost::mutex::scoped_lock l(goals_mutex_);
// add new goal to list of active goals/clients
active_goals_.push_back(goal_info);
}
}
void CartesianImpedanceAction::goalCB(GoalHandle gh) {
// cancel active goal
if (active_goal_.isValid() && (active_goal_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)) {
active_goal_.setCanceled();
}
Goal g = gh.getGoal();
bool valid = true;
for (size_t i = 0; i < g->trajectory.points.size(); i++) {
valid = valid && checkImpedance(g->trajectory.points[i].impedance);
}
if (!valid) {
cartesian_trajectory_msgs::CartesianImpedanceResult res;
res.error_code = cartesian_trajectory_msgs::CartesianImpedanceResult::INVALID_GOAL;
gh.setRejected(res);
}
CartesianImpedanceTrajectory* trj_ptr = new CartesianImpedanceTrajectory;
*trj_ptr = g->trajectory;
CartesianImpedanceTrajectoryConstPtr trj_cptr = CartesianImpedanceTrajectoryConstPtr(trj_ptr);
if (g->trajectory.header.stamp < rtt_rosclock::host_now()) {
valid = false;
cartesian_trajectory_msgs::CartesianImpedanceResult res;
res.error_code = cartesian_trajectory_msgs::CartesianImpedanceResult::OLD_HEADER_TIMESTAMP;
gh.setRejected(res);
}
if (valid) {
port_cartesian_trajectory_command_.write(trj_cptr);
gh.setAccepted();
active_goal_ = gh;
}
}
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 cancelCB(GoalHandle gh){
if (active_goal_ == gh)
{
// Stops the controller.
if(creato){
ROS_INFO_STREAM("Stop thread");
pthread_cancel(trajectoryExecutor);
creato=0;
}
pub_topic.publish(empty);
// Marks the current goal as canceled.
active_goal_.setCanceled();
has_active_goal_ = false;
}
}
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 cancelCB(GoalHandle gh)
{
if (active_goal_ == gh)
{
// stop the real-time motor control
std_srvs::Empty::Request req;
std_srvs::Empty::Response resp;
if(ros::service::exists("stop_motor_output",true))
{
ROS_INFO("Stopping Motor Output");
ros::service::call("stop_motor_output",req,resp);
}
// Marks the current goal as canceled.
active_goal_.setCanceled();
has_active_goal_ = false;
}
}
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 GripperAction::goalCB(GoalHandle gh)
{
// Cancels the currently active goal.
if (has_active_goal_)
{
// Marks the current goal as canceled.
active_goal_.setCanceled();
has_active_goal_ = false;
std::cout << "canceled current active goal" << std::endl;
}
gh.setAccepted();
active_goal_ = gh;
has_active_goal_ = true;
goal_received_ = ros::Time::now();
min_error_seen_ = 1e10;
ROS_INFO_STREAM("Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort);
ROS_INFO("setFingersValues");
std::vector<double> fingerPositionsRadian = jaco_->getFingersJointAngle();
std::cout << "current fingerpositions: " << fingerPositionsRadian[0] << " " << fingerPositionsRadian[1] << " " << fingerPositionsRadian[2] << std::endl;
target_position = active_goal_.getGoal()->command.position;
target_effort = active_goal_.getGoal()->command.max_effort;
//determine if the gripper is supposed to be opened or closed
double current_position = jaco_->getFingersJointAngle()[0];
if(current_position > target_position){
opening = true;
std::cout << "Opening gripper" << std::endl;
}else{
opening = false;
std::cout << "Closing gripper" << std::endl;
}
fingerPositionsRadian[0] = target_position;
fingerPositionsRadian[1] = target_position;
fingerPositionsRadian[2] = target_position;
std::cout << "Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort << std::endl;
double fingerPositionsDegree[3] = {radToDeg(fingerPositionsRadian[0]),radToDeg(fingerPositionsRadian[1]),radToDeg(fingerPositionsRadian[2])};
if(!jaco_->setFingersValues(fingerPositionsDegree))
{
active_goal_.setCanceled();
ROS_ERROR("Cancelling goal: moveJoint didn't work.");
std::cout << "Cancelling goal because setFingers didn't work" << std::endl;
}
last_movement_time_ = ros::Time::now();
}
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);
}
void cancelCB(GoalHandle gh)
{
ROS_ERROR("Action cancel not supported for grasp execution action");
gh.setRejected();
}
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 InternalSpaceSplineTrajectoryAction::goalCB(GoalHandle gh) {
if (!goal_active_) {
trajectory_msgs::JointTrajectory* trj_ptr =
new trajectory_msgs::JointTrajectory;
Goal g = gh.getGoal();
control_msgs::FollowJointTrajectoryResult res;
RTT::Logger::log(RTT::Logger::Debug) << "Received trajectory contain "
<< g->trajectory.points.size()
<< " points" << RTT::endlog();
// fill remap table
for (unsigned int i = 0; i < numberOfJoints_; i++) {
int jointId = -1;
for (unsigned int j = 0; j < g->trajectory.joint_names.size(); j++) {
if (g->trajectory.joint_names[j] == jointNames_[i]) {
jointId = j;
break;
}
}
if (jointId < 0) {
RTT::Logger::log(RTT::Logger::Error)
<< "Trajectory contains invalid joint" << RTT::endlog();
res.error_code =
control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
gh.setRejected(res, "");
return;
} else {
remapTable_[i] = jointId;
}
}
// Sprawdzenie ograniczeń w jointach INVALID_GOAL
bool invalid_goal = false;
for (unsigned int i = 0; i < numberOfJoints_; i++) {
for (int j = 0; j < g->trajectory.points.size(); j++) {
if (g->trajectory.points[j].positions[i] > upperLimits_[remapTable_[i]]
|| g->trajectory.points[j].positions[i]
< lowerLimits_[remapTable_[i]]) {
RTT::Logger::log(RTT::Logger::Debug)
<< "Invalid goal [" << i << "]: " << upperLimits_[remapTable_[i]]
<< ">" << g->trajectory.points[j].positions[i] << ">"
<< lowerLimits_[remapTable_[i]] << RTT::endlog();
invalid_goal = true;
}
}
}
if (invalid_goal) {
RTT::Logger::log(RTT::Logger::Debug)
<< "Trajectory contains invalid goal!" << RTT::endlog();
res.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
gh.setRejected(res, "");
goal_active_ = false;
return;
}
// Remap joints
trj_ptr->header = g->trajectory.header;
trj_ptr->points.resize(g->trajectory.points.size());
for (unsigned int i = 0; i < g->trajectory.points.size(); i++) {
trj_ptr->points[i].positions.resize(
g->trajectory.points[i].positions.size());
for (unsigned int j = 0; j < g->trajectory.points[i].positions.size();
j++) {
trj_ptr->points[i].positions[j] =
g->trajectory.points[i].positions[remapTable_[j]];
}
trj_ptr->points[i].velocities.resize(
g->trajectory.points[i].velocities.size());
for (unsigned int j = 0; j < g->trajectory.points[i].velocities.size();
j++) {
trj_ptr->points[i].velocities[j] =
g->trajectory.points[i].velocities[remapTable_[j]];
}
trj_ptr->points[i].accelerations.resize(
g->trajectory.points[i].accelerations.size());
for (unsigned int j = 0; j < g->trajectory.points[i].accelerations.size();
j++) {
trj_ptr->points[i].accelerations[j] = g->trajectory.points[i]
.accelerations[remapTable_[j]];
}
trj_ptr->points[i].time_from_start = g->trajectory.points[i]
.time_from_start;
}
// Sprawdzenie czasu w nagłówku OLD_HEADER_TIMESTAMP
if (g->trajectory.header.stamp < rtt_rosclock::host_now()) {
RTT::Logger::log(RTT::Logger::Debug) << "Old header timestamp"
<< RTT::endlog();
res.error_code =
control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP;
gh.setRejected(res, "");
}
trajectory_finish_time_ = g->trajectory.header.stamp
+ g->trajectory.points[g->trajectory.points.size() - 1].time_from_start;
activeGoal_ = gh;
goal_active_ = true;
bool ok = true;
RTT::TaskContext::PeerList peers = this->getPeerList();
for (size_t i = 0; i < peers.size(); i++) {
RTT::Logger::log(RTT::Logger::Debug) << "Starting peer : " << peers[i]
<< RTT::endlog();
ok = ok && this->getPeer(peers[i])->start();
}
if (ok) {
trajectory_msgs::JointTrajectoryConstPtr trj_cptr =
trajectory_msgs::JointTrajectoryConstPtr(trj_ptr);
trajectory_ptr_port_.write(trj_cptr);
gh.setAccepted();
goal_active_ = true;
} else {
gh.setRejected();
goal_active_ = false;
}
} else {
gh.setRejected();
}
}
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 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 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;
}
}
*/
}
