//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 executeCB(const tilting_servo::servoGoalConstPtr &goal)
{
if(start == true)
{
servo_move(Comport, servo_id, goal->angle, goal->speed);
prev_goal = goal->angle;
start = false;
}
if(as_.isNewGoalAvailable())
as_.acceptNewGoal();
if(goal->angle != prev_goal || goal->speed != prev_speed)
{
servo_move(Comport, servo_id, goal->angle, goal->speed);
prev_goal = goal->angle;
prev_speed = goal->speed;
}
while((as_.isNewGoalAvailable() == false) && ros::ok())
{
feedback_.angle = read_angle(Comport, servo_id);
if(feedback_.angle >= min_angle - 10 && feedback_.angle <= max_angle + 10)
as_.publishFeedback(feedback_);
}
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
// set the action state to preempted
as_.setPreempted();
}
}
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;
}
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 execute_callback(const behavior_tree_core::BTGoalConstPtr &goal)
{
// publish info to the console for the user
ROS_INFO("Starting Action");
// start executing the action
int i = 0;
while (i < 5)
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested())
{
ROS_INFO("Action Halted");
// set the action state to preempted
as_.setPreempted();
break;
}
ROS_INFO("Executing Action");
ros::Duration(0.5).sleep(); // waiting for 0.5 seconds
i++;
}
if (i == 5)
{
set_status(SUCCESS);
}
}
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 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();
}
}
/*!
* \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!");
}
}
//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
}
// Go to this pose
bool go_home(tf::Pose& pose_) {
tf::Vector3 start_p(pose_.getOrigin());
tf::Quaternion start_o(pose_.getRotation());
msg_pose.pose.position.x = start_p.x();
msg_pose.pose.position.y = start_p.y();
msg_pose.pose.position.z = start_p.z();
msg_pose.pose.orientation.w = start_o.w();
msg_pose.pose.orientation.x = start_o.x();
msg_pose.pose.orientation.y = start_o.y();
msg_pose.pose.orientation.z = start_o.z();
pub_.publish(msg_pose);
sendNormalForce(0);
ros::Rate thread_rate(2);
ROS_INFO("Homing...");
while(ros::ok()) {
double oerr =(ee_pose.getRotation() - start_o).length() ;
double perr = (ee_pose.getOrigin() - start_p).length();
feedback_.progress = 0.5*(perr+oerr);
as_.publishFeedback(feedback_);
ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
if(perr< 0.02 && oerr < 0.02) {
break;
}
if (as_.isPreemptRequested() || !ros::ok() )
{
sendNormalForce(0);
sendPose(ee_pose);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
thread_rate.sleep();
}
return ros::ok();
}
// move platform server receives a new goal
void moveGoalCB()
{
set_terminal_state_ = true;
move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal;
ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq);
if (as_.isPreemptRequested() ||!ros::ok())
{
ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq);
if (planning_)
ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
if (controlling_)
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.result_state = 0;
move_result_.error_string = "Preempt Requested!!!";
as_.setPreempted(move_result_);
return;
}
// Convert move goal to plan goal
pose_goal_.pose_goal = move_goal_.nav_goal;
if (planner_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down");
planning_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Planner is down";
as_.setAborted(move_result_);
}
else
{
ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback());
planning_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner");
}
return;
}
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 rm_multi_mapper_db::G2oWorkerGoalConstPtr & goal) {
ros::Rate r(1);
bool success = true;
if (as_.isPreemptRequested() || !ros::ok()) {
ROS_INFO("%s: Preempted", action_name_.c_str());
as_.setPreempted();
success = false;
}
for (size_t i = 0; i < goal->Overlap.size(); i++) {
pcl::PointCloud<pcl::PointXYZ> keypoints3d1, keypoints3d2;
cv::Mat descriptors1, descriptors2;
//std::cerr << "Trying to match " << goal->Overlap[i].first << " "
// << goal->Overlap[i].second << std::endl;
U->get_keypoints(goal->Overlap[i].first, keypoints3d1, descriptors1);
U->get_keypoints(goal->Overlap[i].second, keypoints3d2,
descriptors2);
Sophus::SE3f t;
if (U->find_transform(keypoints3d1, keypoints3d2, descriptors1,
descriptors2, t)) {
U->add_measurement(goal->Overlap[i].first,
goal->Overlap[i].second, t, "RANSAC");
}
}
std::cout << "Done";
if (success) {
ROS_INFO("%s: Succeeded", action_name_.c_str());
result_.reply = true;
as_.setSucceeded(result_);
}
}
void executeCB(const learning_actionlib::FibonacciGoalConstPtr& goal) {
ros::Rate r(1);
bool success = true;
feedback_.sequence.clear();
feedback_.sequence.push_back(0);
feedback_.sequence.push_back(1);
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]);
for (int i = 1; i <= goal->order; i++) {
if (as_.isPreemptRequested() || !ros::ok()) {
ROS_INFO("%s: Preempted", action_name_.c_str());
as_.setPreempted();
success = false;
break;
}
feedback_.sequence.push_back(feedback_.sequence[i] +
feedback_.sequence[i - 1]);
as_.publishFeedback(feedback_);
r.sleep();
}
if (success) {
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
as_.setSucceeded(result_);
}
}
void executeCB(const lwr_peg_in_hole::RobotMoveGoalConstPtr& goal)
{
robot_moved_ = false;
ros::Timer mover_thread = nh_.createTimer(ros::Duration(0.001),
boost::bind(&RobotMoveActionServer::moveBinCB, this, goal, _1), true);
ros::Rate r(30);
while (ros::ok()) {
if(robot_moved_) {
ROS_INFO("RobotMove action complete.");
result_.success = true;
act_srv_.setSucceeded(result_);
return;
}
if(act_srv_.isPreemptRequested()) {
robot_move_.stopJointTrajectory();
ROS_INFO("Preempting robot move");
mover_thread.stop();
return;
}
ros::spinOnce();
r.sleep();
}
}
// add training images for a person
void addTrainingImagesCb(const sensor_msgs::ImageConstPtr& msg) {
// cout << "addTrainingImagesCb" << endl;
if (_as.isPreemptRequested() || !ros::ok()) {
// std::cout << "preempt req or not ok" << std::endl;
ROS_INFO("%s: Preempted", _action_name.c_str());
// set the action state to preempted
_as.setPreempted();
// success = false;
// break;
// cout << "shutting down _image_sub" << endl;
_image_sub.shutdown();
return;
}
if (!_as.isActive()) {
// std::cout << "not active" << std::endl;
// cout << "shutting down _image_sub" << endl;
_image_sub.shutdown();
return;
}
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("%s: cv_bridge exception: %s", _action_name.c_str(), e.what());
return;
}
if (_window_rows == 0) {
_window_rows = cv_ptr->image.rows;
_window_cols = cv_ptr->image.cols;
}
std::vector<cv::Rect> faces;
// _fd.detectFaces(cv_ptr->image, faces, true);
std::vector<cv::Mat> faceImgs = _fd.getFaceImgs(cv_ptr->image, faces, true);
if (faceImgs.size() > 0)
_fc->capture(faceImgs[0]);
// call images capturing function
_result.names.push_back(_goal_argument);
// _result.confidence.push_back(2.0);
int no_images_to_click;
_ph.getParam("no_training_images", no_images_to_click);
if (_fc->getNoImgsClicked() >= no_images_to_click) {
// Mat inactive = cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_32F);
Mat inactive(_window_rows, _window_cols, CV_8UC3, CV_RGB(20,20,20));
appendStatusBar(inactive, "INACTIVE", "Images added. Please train.");
cv::imshow(_windowName, inactive);
// cv::displayOverlay(_windowName, "Images added", 3000);
_as.setSucceeded(_result);
} else {
// update GUI window
// check GUI parameter
appendStatusBar(cv_ptr->image, "ADDING IMAGES.", "Images added");
cv::imshow(_windowName, cv_ptr->image);
}
cv::waitKey(3);
}
void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) {
ROS_INFO("in executeCB: received manipulation task");
goal_action_code_ = goal->action_code; //verbatim from received goal
action_code_ = goal->action_code; //init: this value changes as state machine advances through steps
ROS_INFO("requested action code is: %d", goal_action_code_);
//if action code is "MANIP_OBJECT", need to go through a sequence of action codes
//otherwise, action code is a simple action code, and can use it as-is
/*
if (goal_action_code_ == coordinator::ManipTaskGoal::MANIP_OBJECT) {
//if command is for manip, then we can expect an object code, perception source and dropoff pose
object_code_ = goal->object_code; //what type of object is this?
perception_source_ = goal->perception_source; //name sensor or provide coords
dropoff_pose_ = goal->dropoff_frame;
ROS_INFO("object code is: %d", object_code_);
ROS_INFO("perception_source is: %d", goal->perception_source);
//if (object_code_ == coordinator::ManipTaskGoal::TOY_BLOCK) {
if (object_code_ == ObjectIdCodes::TOY_BLOCK_ID) {
vision_object_code_ = object_code_; //same code;
ROS_INFO("using object-finder object code %d", vision_object_code_);
pickup_action_code_ = object_grabber::object_grabberGoal::GRAB_TOY_BLOCK;
dropoff_action_code_ = object_grabber::object_grabberGoal::PLACE_TOY_BLOCK;
//start the state machine with perceptual processing task:
action_code_ = coordinator::ManipTaskGoal::GET_PICKUP_POSE;
} else {
ROS_WARN("unknown object type in manipulation action");
as_.setAborted(result_);
}
} else if (goal_action_code_ == coordinator::ManipTaskGoal::DROPOFF_OBJECT) {
object_code_ = goal->object_code; //what type of object is this?
*/
if (goal_action_code_ == coordinator::ManipTaskGoal::DROPOFF_OBJECT) {
object_code_ = goal->object_code; //what type of object is this?
} else if (goal_action_code_ == coordinator::ManipTaskGoal::GRAB_OBJECT) {
object_code_ = goal->object_code; //what type of object is this?
ROS_INFO("object code is: %d", object_code_);
if (goal->perception_source == coordinator::ManipTaskGoal::BLIND_MANIP) {
ROS_INFO("blind manipulation; using provided pick-up pose");
pickup_pose_ = goal->pickup_frame;
}
} else if (goal_action_code_ == coordinator::ManipTaskGoal::GET_PICKUP_POSE) {
ROS_INFO("object code is: %d", object_code_);
ROS_INFO("perception_source is: %d", goal->perception_source);
object_code_ = goal->object_code; //what type of object is this?
perception_source_ = goal->perception_source; //name sensor or provide coords
vision_object_code_ = object_code_; //same code
}
status_code_ = coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK; //coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
working_on_task_ = true;
//do work here
while (working_on_task_) { //coordinator::ManipTaskResult::MANIP_SUCCESS) {
feedback_.feedback_status = status_code_;
as_.publishFeedback(feedback_);
//ROS_INFO("executeCB: status_code = %d", status_code_);
// each iteration, check if cancellation has been ordered
if (as_.isPreemptRequested()) {
ROS_WARN("goal cancelled!");
result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
status_code_ = coordinator::ManipTaskFeedback::ABORTED;
working_on_task_ = false;
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 (action_code_) {
case coordinator::ManipTaskGoal::FIND_TABLE_SURFACE:
ROS_INFO("serving request to find table surface");
found_object_code_ = object_finder::objectFinderResult::OBJECT_FINDER_BUSY;
object_finder_goal_.object_id = ObjectIdCodes::TABLE_SURFACE;
//vision_object_code_;
object_finder_goal_.known_surface_ht = false; //require find table height
//object_finder_goal_.surface_ht = 0.05; //this is ignored for known_surface_ht=false
object_finder_ac_.sendGoal(object_finder_goal_,
boost::bind(&TaskActionServer::objectFinderDoneCb_, this, _1, _2));
action_code_ = coordinator::ManipTaskGoal::WAIT_FIND_TABLE_SURFACE;
ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_);
ROS_INFO("waiting on perception");
break;
case coordinator::ManipTaskGoal::WAIT_FIND_TABLE_SURFACE:
if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FOUND) {
ROS_INFO("surface-finder success");
surface_height_ = pickup_pose_.pose.position.z; // table-top height, as found by object_finder
found_surface_height_ = true;
ROS_INFO("found table ht = %f", surface_height_);
as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
return; //done w/ callback
} else if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FINDER_BUSY) {
//ROS_INFO("waiting on perception"); //do nothing
} else {
ROS_WARN("object-finder failure; aborting");
action_code_ = coordinator::ManipTaskGoal::ABORT;
result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PERCEPTION;
found_surface_height_ = false;
}
break;
case coordinator::ManipTaskGoal::GET_PICKUP_POSE:
ROS_INFO("establishing pick-up pose");
if (perception_source_ == coordinator::ManipTaskGoal::BLIND_MANIP) {
ROS_INFO("blind manipulation; using provided pick-up pose");
pickup_pose_ = goal->pickup_frame;
result_.object_pose = pickup_pose_;
//done with perception, but do fake waiting anyway
//declare victor on finding object
found_object_code_ = object_finder::objectFinderResult::OBJECT_FOUND;
action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_FINDER;
status_code_ = coordinator::ManipTaskFeedback::PERCEPTION_BUSY;
} else if (perception_source_ == coordinator::ManipTaskGoal::PCL_VISION) {
ROS_INFO("invoking object finder");
found_object_code_ = object_finder::objectFinderResult::OBJECT_FINDER_BUSY;
ROS_INFO("instructing finder to locate object %d", vision_object_code_);
object_finder_goal_.object_id = vision_object_code_;
if (found_surface_height_) {
object_finder_goal_.known_surface_ht = true;
object_finder_goal_.surface_ht = surface_height_;
ROS_INFO("using surface ht = %f", surface_height_);
} else {
object_finder_goal_.known_surface_ht = false; //require find table height
//object_finder_goal_.surface_ht = 0.05; //not needed
}
ROS_INFO("sending object-finder goal: ");
object_finder_ac_.sendGoal(object_finder_goal_,
boost::bind(&TaskActionServer::objectFinderDoneCb_, this, _1, _2));
action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_FINDER;
ROS_INFO("waiting on perception");
} else {
ROS_WARN("unrecognized perception mode; quitting");
action_code_ = coordinator::ManipTaskGoal::ABORT;
result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PERCEPTION;
}
ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_);
break;
case coordinator::ManipTaskGoal::WAIT_FOR_FINDER:
if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FOUND) {
ROS_INFO("object-finder success");
//next step: use the pose to grab object:
/* if (goal_action_code_ == coordinator::ManipTaskGoal::MANIP_OBJECT) {
action_code_ = coordinator::ManipTaskGoal::GRAB_OBJECT;
status_code_ = coordinator::ManipTaskFeedback::DROPOFF_PLANNING_BUSY;
} else*/ {
working_on_task_ = false; // test--set to goal achieved
action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
status_code_ = coordinator::ManipTaskFeedback::COMPLETED_MOVE;
result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
//object pose is in result message
as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
return; //done w/ callback
}
//will later test for result code of object grabber, so initialize it to PENDING
//(next step in state machine)
object_grabber_return_code_ = object_grabber::object_grabberResult::PENDING;
} else if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FINDER_BUSY) {
//ROS_INFO("waiting on perception"); //continue waiting
} else {
ROS_WARN("object-finder failure; aborting");
action_code_ = coordinator::ManipTaskGoal::ABORT;
result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PERCEPTION;
}
break;
case coordinator::ManipTaskGoal::GRAB_OBJECT:
status_code_ = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY;
ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_);
//ros::Duration(2.0).sleep();
//if here, then presumably have a valid pose for object of interest; grab it!
//send object-grabber action code to grab specified object
object_grabber_goal_.action_code = object_grabber::object_grabberGoal::GRAB_OBJECT;//pickup_action_code_; //specify the object to be grabbed
object_grabber_goal_.object_frame = pickup_pose_; //and the object's current pose
object_grabber_goal_.object_id = object_code_; // = goal->object_code;
object_grabber_goal_.grasp_option = object_grabber::object_grabberGoal::DEFAULT_GRASP_STRATEGY; //from above
ROS_INFO("sending goal to grab object: ");
object_grabber_ac_.sendGoal(object_grabber_goal_,
boost::bind(&TaskActionServer::objectGrabberDoneCb_, this, _1, _2));
action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_GRAB_OBJECT;
status_code_ = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY;
//will inspect this status to see if object grasp is eventually successful
object_grabber_return_code_ = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY;
break;
case coordinator::ManipTaskGoal::WAIT_FOR_GRAB_OBJECT:
if (object_grabber_return_code_ == object_grabber::object_grabberResult::OBJECT_ACQUIRED) { //success!
ROS_INFO("acquired object");
/*if (goal_action_code_ == coordinator::ManipTaskGoal::MANIP_OBJECT) {
//for manip command, have more steps to complete:
action_code_ = coordinator::ManipTaskGoal::DROPOFF_OBJECT;
status_code_ = coordinator::ManipTaskFeedback::PICKUP_SUCCESSFUL;
} else*/ { //if just a grab command, we are now done
working_on_task_ = false; // test--set to goal achieved
action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
status_code_ = coordinator::ManipTaskFeedback::COMPLETED_MOVE;
result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
//object pose is in result message
as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
return; //done w/ callback
}
} else if (object_grabber_return_code_ == object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY) {
// do nothing--just wait patiently
//ROS_INFO("waiting for object grab");
} else {
ROS_WARN("trouble with acquiring object");
action_code_ = coordinator::ManipTaskGoal::ABORT;
result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PICKUP;
}
break;
case coordinator::ManipTaskGoal::DROPOFF_OBJECT:
status_code_ = coordinator::ManipTaskFeedback::DROPOFF_MOTION_BUSY; //coordinator::ManipTaskResult::MANIP_SUCCESS; //code 0
ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_);
object_grabber_goal_.action_code = object_grabber::object_grabberGoal::DROPOFF_OBJECT;//dropoff_action_code_; //specify the object to be grabbed
object_grabber_goal_.object_id = object_code_; // = goal->object_code;
dropoff_pose_= goal->dropoff_frame;
object_grabber_goal_.object_frame = dropoff_pose_; //and the object's current pose
object_grabber_goal_.grasp_option = object_grabber::object_grabberGoal::DEFAULT_GRASP_STRATEGY; //from above
ROS_INFO("sending goal to drop off object: ");
object_grabber_ac_.sendGoal(object_grabber_goal_,
boost::bind(&TaskActionServer::objectGrabberDoneCb_, this, _1, _2));
action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_DROPOFF_OBJECT;
//will inspect this status to see if object grasp is eventually successful
object_grabber_return_code_ = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY;
break;
case coordinator::ManipTaskGoal::WAIT_FOR_DROPOFF_OBJECT:
//ROS_INFO("object_grabber_return_code_ = %d",object_grabber_return_code_);
if (object_grabber_return_code_ == object_grabber::object_grabberResult::SUCCESS) { //success!
ROS_INFO("switch/case happiness! dropped off object; manip complete");
working_on_task_ = false; // test--set to goal achieved
action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
status_code_ = coordinator::ManipTaskFeedback::COMPLETED_DROPOFF;
result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
return; //done w/ callback
} else if (object_grabber_return_code_ == object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY) {
// do nothing--just wait patiently
//ROS_INFO("waiting for object dropoff");
} else {
ROS_WARN("trouble with acquiring object");
action_code_ = coordinator::ManipTaskGoal::ABORT;
result_.manip_return_code = coordinator::ManipTaskResult::FAILED_DROPOFF;
}
break;
case coordinator::ManipTaskGoal::MOVE_TO_PRE_POSE:
status_code_ = coordinator::ManipTaskFeedback::PREPOSE_MOVE_BUSY;
object_grabber_goal_.action_code = object_grabber::object_grabberGoal::MOVE_TO_WAITING_POSE; //specify the object to be grabbed
ROS_INFO("sending goal to move to pre-pose: ");
object_grabber_ac_.sendGoal(object_grabber_goal_,
boost::bind(&TaskActionServer::objectGrabberDoneCb_, this, _1, _2));
action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_MOVE_TO_PREPOSE;
//will inspect this status to see if object grasp is eventually successful
object_grabber_return_code_ = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY;
break;
case coordinator::ManipTaskGoal::WAIT_FOR_MOVE_TO_PREPOSE:
if (object_grabber_return_code_ == object_grabber::object_grabberResult::SUCCESS) { //success!
ROS_INFO("completed move to pre-pose");
working_on_task_ = false; // test--set to goal achieved
action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
status_code_ = coordinator::ManipTaskFeedback::COMPLETED_MOVE;
result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
return; //done w/ callback
}
else {
ROS_INFO("object_grabber_return_code_ = %d",object_grabber_return_code_);
ros::Duration(0.5).sleep();
}
break;
case coordinator::ManipTaskGoal::ABORT:
ROS_WARN("aborting goal...");
//retain reason for failure to report back to client
//result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK;
status_code_ = coordinator::ManipTaskFeedback::ABORTED;
working_on_task_ = false;
as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
return; // done with callback
default:
ROS_WARN("executeCB: error--case not recognized");
working_on_task_ = false;
break;
}
}
ROS_INFO("executeCB: I should not be here...");
//if we survive to here, then the goal was successfully accomplished; inform the client
result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
as_.setAborted(result_); // return the "result" message to client, along with "success" status
return;
}
int executeCB(const soft_move_base::SoftMoveBaseGoalConstPtr &goal)
{
ROS_INFO("Received new trajectory");
SM_TRAJ tmpTraj;
ros::Rate _loop_rate(1/_dt);
_timeFromStart= 0;
_tmpTimeFromStart= 0;
_timer = 0;
loadNewTraj(goal->traj);
double duration = 0.0;
duration = _newTraj.getDuration();
limits.clear();
for(int i=0; i<6; ++i)
{
limit.maxJerk = _newTraj.jmax[i] * 1.1;
limit.maxAcc = _newTraj.amax[i] * 1.1;
limit.maxVel = _newTraj.vmax[i] * 1.1;
limits.push_back(limit);
}
//double t = 0;
//FILE* file;
//file = fopen("baseExperiment.dat", "w");
bool cont = true;
do {
// end if goal canceled
if( as_.isPreemptRequested())
break;
getNextRobotCond(_timeFromStart + _dt);
//printCond(_nextRobotCond_w);
getPoseInRobotFrame(_nextRobotCond_w);
//printCond(_nextRobotCond_r);
_currRobotCond_r[0].x = 0;
_currRobotCond_r[0].v = _lastVel[0];
_currRobotCond_r[0].a = _lastAcc[0];
_currRobotCond_r[1].x = 0;
_currRobotCond_r[1].v = _lastVel[1];
_currRobotCond_r[1].a = _lastAcc[1];
_currRobotCond_r[5].x = 0;
_currRobotCond_r[5].v = _lastVel[5];
_currRobotCond_r[5].a = _lastAcc[5];
//printCond(_currRobotCond_r);
ROS_INFO("TMP TIME:%f", _tmpTimeFromStart);
if( _timeFromStart == 0.0)
{
//correction
tmpTraj.clear();
boundsCond(_currRobotCond_r);
boundsCond(_nextRobotCond_r);
tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
_tmpTimeFromStart = _dt;
//printCond( _newRobotCond_r);
}
else if(_timer - 0.5 >= 0.0001
|| _tmpTimeFromStart > tmpTraj.getDuration())
{
_timer = 0;
//correction
tmpTraj.clear();
tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
_tmpTimeFromStart = _dt;
}
tmpTraj.getMotionCond(_tmpTimeFromStart,_newRobotCond_r);
_lastVel[0]= _newRobotCond_r[0].v;
_lastVel[1]= _newRobotCond_r[1].v;
_lastVel[5]= _newRobotCond_r[5].v;
_lastAcc[0]= _newRobotCond_r[0].a;
_lastAcc[1]= _newRobotCond_r[1].a;
_lastAcc[5]= _newRobotCond_r[5].a;
//printCond(_newRobotCond_r);
publishNewCond(_newRobotCond_r);
_timeFromStart += _dt;
_tmpTimeFromStart += _dt;
_timer += _dt;
if(_timeFromStart > _newTraj.getDuration())
_timeFromStart = _newTraj.getDuration();
_loop_rate.sleep();
if(_timeFromStart >= _newTraj.getDuration())
{
cont = fabs(_newRobotCond_r[0].x) > _tolerance[0]
|| fabs(_newRobotCond_r[1].x) > _tolerance[1]
|| fabs(_newRobotCond_r[5].x) > _tolerance[2];
}
ros::spinOnce();
} while(cont);
soft_move_base::SoftMoveBaseResult result;
result.result = true;
as_.setSucceeded(result);
return 0;
}
void run()
{
if(executing_)
{
failure_ = false;
watchdog_counter = 0;
//if (as_follow_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
if (!ros::ok() || current_operation_mode_ != "velocity")
{
// set the action state to preempted
executing_ = false;
traj_generator_->isMoving = false;
//as_.setPreempted();
failure_ = true;
return;
}
if (as_follow_.isPreemptRequested())
{
//as_follow_.setAborted()
failure_ = true;
preemted_ = true;
ROS_INFO("Preempted trajectory action");
return;
}
std::vector<double> des_vel;
if(traj_generator_->step(q_current, des_vel))
{
if(!traj_generator_->isMoving) //Finished trajectory
{
executing_ = false;
preemted_ = false;
}
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(DOF);
for(int i=0; i<DOF; i++)
{
target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = des_vel.at(i);
}
//send everything
joint_vel_pub_.publish(target_joint_vel);
}
else
{
ROS_INFO("An controller error occured!");
failure_ = true;
executing_ = false;
}
}
else
{ //WATCHDOG TODO: don't always send
if(watchdog_counter < 10)
{
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(DOF);
for (int i = 0; i < DOF; i += 1)
{
target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = 0;
}
joint_vel_pub_.publish(target_joint_vel);
ROS_INFO("Publishing 0-vel (%d)", DOF);
}
watchdog_counter++;
}
}
void timer_callback(const ros::TimerEvent &)
{
mil_msgs::MoveToResult actionresult;
// Handle disabled, killed, or no odom before attempting to produce trajectory
std::string err = "";
if (disabled)
err = "c3 disabled";
else if (kill_listener.isRaised())
err = "killed";
else if (!c3trajectory)
err = "no odom";
if (!err.empty())
{
if (c3trajectory)
c3trajectory.reset(); // On revive/enable, wait for odom before station keeping
// Cancel all goals while killed/disabled/no odom
if (actionserver.isNewGoalAvailable())
actionserver.acceptNewGoal();
if (actionserver.isActive())
{
actionresult.error = err;
actionresult.success = false;
actionserver.setAborted(actionresult);
}
return;
}
ros::Time now = ros::Time::now();
auto old_waypoint = current_waypoint;
if (actionserver.isNewGoalAvailable())
{
boost::shared_ptr<const mil_msgs::MoveToGoal> goal = actionserver.acceptNewGoal();
current_waypoint =
subjugator::C3Trajectory::Waypoint(Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist),
goal->speed, !goal->uncoordinated, !goal->blind);
current_waypoint_t = now;
this->linear_tolerance = goal->linear_tolerance;
this->angular_tolerance = goal->angular_tolerance;
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::GREEN);
// Check if waypoint is valid
std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid(
Pose_from_Waypoint(current_waypoint), current_waypoint.do_waypoint_validation);
actionresult.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second);
actionresult.success = checkWPResult.first;
if (checkWPResult.first == false && waypoint_check_) // got a point that we should not move to
{
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::RED);
if (checkWPResult.second ==
WAYPOINT_ERROR_TYPE::UNKNOWN) // if unknown, check if there's a huge displacement with the new waypoint
{
auto a_point = Pose_from_Waypoint(current_waypoint);
auto b_point = Pose_from_Waypoint(old_waypoint);
// If moved more than .5m, then don't allow
if (abs(a_point.position.x - b_point.position.x) > .5 || abs(a_point.position.y - b_point.position.y) > .5)
{
ROS_ERROR("can't move there! - need to rotate");
current_waypoint = old_waypoint;
}
}
// if point is occupied, reject move
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED)
{
ROS_ERROR("can't move there! - waypoint is occupied");
current_waypoint = old_waypoint;
}
// if point is above water, reject move
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::ABOVE_WATER)
{
ROS_ERROR("can't move there! - waypoint is above water");
current_waypoint = old_waypoint;
}
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::NO_OGRID)
{
ROS_ERROR("WaypointValidity - Did not recieve any ogrid");
}
}
}
if (actionserver.isPreemptRequested())
{
current_waypoint = c3trajectory->getCurrentPoint();
current_waypoint.do_waypoint_validation = false;
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;
}
// Remember the previous trajectory
auto old_trajectory = c3trajectory->getCurrentPoint();
while (c3trajectory_t + traj_dt < now)
{
c3trajectory->update(traj_dt.toSec(), current_waypoint, (c3trajectory_t - current_waypoint_t).toSec());
c3trajectory_t += traj_dt;
}
// Check if we will hit something while in trajectory the new trajectory
geometry_msgs::Pose traj_point; // Convert messages to correct type
auto p = c3trajectory->getCurrentPoint();
traj_point.position = vec2xyz<Point>(p.q.head(3));
quaternionTFToMsg(tf::createQuaternionFromRPY(p.q[3], p.q[4], p.q[5]), traj_point.orientation);
std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult =
waypoint_validity_.is_waypoint_valid(Pose_from_Waypoint(p), c3trajectory->do_waypoint_validation);
if (checkWPResult.first == false && checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED && waypoint_check_)
{ // New trajectory will hit an occupied goal, so reject
ROS_ERROR("can't move there! - bad trajectory");
current_waypoint = old_trajectory;
current_waypoint.do_waypoint_validation = false;
current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities
current_waypoint_t = now;
c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
c3trajectory_t = now;
actionresult.success = false;
actionresult.error = WAYPOINT_ERROR_TO_STRING.at(WAYPOINT_ERROR_TYPE::OCCUPIED_TRAJECTORY);
}
PoseTwistStamped msg;
msg.header.stamp = c3trajectory_t;
msg.header.frame_id = fixed_frame;
msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
trajectory_pub.publish(msg);
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(c3trajectory->getCurrentPoint()), 200);
PoseStamped msgVis;
msgVis.header = msg.header;
msgVis.pose = msg.posetwist.pose;
trajectory_vis_pub.publish(msgVis);
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())
{
actionresult.error = "";
actionresult.success = true;
actionserver.setSucceeded(actionresult);
}
}
// Do stuff with learned models
// Phase - Reach, Roll or Back?
// Dynamics type - to select the type of master/slave dynamics (linear/model etc.)
// reachingThreshold - you know
// model_dt - you know
bool learned_model_execution(DoughTaskPhase phase, CDSController::DynamicsType masterType, CDSController::DynamicsType slaveType, double reachingThreshold, double model_dt, tf::Transform trans_obj, tf::Transform trans_att, std::string model_base_path, std::string force_gmm_id) {
ROS_INFO_STREAM(" Model Path "<<model_base_path);
ROS_INFO_STREAM(" Learned model execution with phase "<<phase);
ROS_INFO_STREAM(" Reaching threshold "<<reachingThreshold);
ROS_INFO_STREAM(" Model DT "<<model_dt);
if (force_gmm_id!="")
ROS_INFO_STREAM(" Force GMM ID: "<< force_gmm_id);
ros::Rate wait(1);
tf::Transform trans_final_target, ee_pos_att;
// To Visualize EE Frames
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
br.sendTransform(tf::StampedTransform(trans_obj, ros::Time::now(), world_frame, "/object_frame"));
// convert attractor information to world frame
trans_final_target.mult(trans_obj, trans_att);
ROS_INFO_STREAM("Final target origin "<<trans_final_target.getOrigin().getX()<<","<<trans_final_target.getOrigin().getY()<<","<<trans_final_target.getOrigin().getZ());
ROS_INFO_STREAM("Final target orient "<<trans_final_target.getRotation().getX()<<","<<trans_final_target.getRotation().getY()<<","<<trans_final_target.getRotation().getZ()<<","<<trans_final_target.getRotation().getW());
// Publish attractors if running in simulation or with fixed values
if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
}
// Initialize CDS
CDSExecution *cdsRun = new CDSExecution;
cdsRun->initSimple(model_base_path, phase, force_gmm_id);
cdsRun->setObjectFrame(toMatrix4(trans_obj));
cdsRun->setAttractorFrame(toMatrix4(trans_att));
cdsRun->setCurrentEEPose(toMatrix4(ee_pose));
cdsRun->setDT(model_dt);
if (phase==PHASEBACK || phase==PHASEROLL)
masterType = CDSController::LINEAR_DYNAMICS;
// Roll slow but move fast for reaching and back phases.
// If models have proper speed, this whole block can go!
if(phase == PHASEROLL) {
//cdsRun->setMotionParameters(1,1,0.5,reachingThreshold, masterType, slaveType);
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
// large threshold to avoid blocking forever
// TODO: should rely on preempt in action client.
// reachingThreshold = 0.02;
reachingThreshold = 0.025;
} else {
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
}
cdsRun->postInit();
// If phase is rolling, need force model as well
GMR* gmr_perr_force = NULL;
if(phase == PHASEROLL) {
gmr_perr_force = getNewGMRMappingModel(model_base_path, force_gmm_id);
if(!gmr_perr_force) {
ROS_ERROR("Cannot initialize GMR model");
return false;
}
}
ros::Duration loop_rate(model_dt);
tf::Pose mNextRobotEEPose = ee_pose;
std::vector<double> gmr_in, gmr_out;
gmr_in.resize(1);gmr_out.resize(1);
double prog_curr, full_err, pos_err, ori_err, new_err, gmr_err;
tf::Vector3 att_t, curr_t;
ROS_INFO("Execution started");
tf::Pose p;
bool bfirst = true;
std_msgs::String string_msg, action_name_msg, model_fname_msg;
std::stringstream ss, ss_model;
if(phase == PHASEREACH) {
ss << "reach ";
}
else if (phase == PHASEROLL){
ss << "roll";
}
else if (phase == PHASEBACK){
ss << "back";
}
if (!homing){
string_msg.data = ss.str();
pub_action_state_.publish(string_msg);
// ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
if (force_gmm_id=="")
ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
else
ss_model << path_matlab_plot << "/Phase" << phase << "/ForceProfile_" << force_gmm_id << ".fig";
//ss_model << "/Phase" << phase << "/masterGMM.fig";
model_fname_msg.data = ss_model.str();
pub_model_fname_.publish(model_fname_msg);
action_name_msg.data = ss.str();
pub_action_name_.publish(action_name_msg);
plot_dyn = 1;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
}
while(ros::ok()) {
if (!homing)
plot_dyn = 1;
else
plot_dyn = 0;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
// Nadia's stuff
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
pos_err = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
//Real Orientation Error qdiff = acos(dot(q1_norm,q2_norm))*180/pi
ori_err = acos(abs(trans_final_target.getRotation().dot(ee_pose.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Position Threshold : " << reachingThreshold << " ... Current Error: "<<pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Orientation Threshold : " << 0.01 << " ... Current Error: "<<ori_err);
/*double att_pos_err = (trans_final_target.getOrigin() - p.getOrigin()).length();
double att_ori_err = acos(abs(trans_final_target.getRotation().dot(p.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Position Error: " << att_pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Orientation Error: " << att_ori_err);*/
switch (phase) {
// Home, reach and back are the same control-wise
case PHASEREACH:
case PHASEBACK:
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
prog_curr = 0.5*((trans_final_target.getOrigin() - ee_pose.getOrigin()).length() + (trans_final_target.getRotation()-ee_pose.getRotation()).length());
// Compute Next Desired EE Pose
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Aswhini's Hack! Dont rely on model's orientation interpolation. Set it equal to target orientation to avoid
// going the wrong way around
p.setRotation(trans_final_target.getRotation());
//Publish desired force
gmr_msg.data = 0.0;
pub_gmr_out_.publish(gmr_msg);
break;
case PHASEROLL:
// Current progress in rolling phase is simply the position error
prog_curr = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
// New position error being fed to GMR Force Model
att_t = tf::Vector3(trans_final_target.getOrigin().getX(),trans_final_target.getOrigin().getY(),0.0);
curr_t = tf::Vector3(ee_pose.getOrigin().getX(),ee_pose.getOrigin().getY(),0.0);
new_err = (att_t - curr_t).length();
gmr_err = new_err;
//Hack! Truncate errors to corresponding models
if ((strncmp(force_gmm_id.c_str(),"first",5)==0) && (new_err > 0.03)){
gmr_err = 0.03;
}
if ((strncmp(force_gmm_id.c_str(),"mid",3)==0) && (new_err > 0.07)){
gmr_err = 0.07;
}
if ((strncmp(force_gmm_id.c_str(),"last",4)==0) && (new_err > 0.13)){
gmr_err = 0.13;
}
//pos_err = prog_curr;
ori_err = 0;
gmr_err = gmr_err;
gmr_in[0] = gmr_err; // distance between EE and attractor
// Query the model for desired force
getGMRResult(gmr_perr_force, gmr_in, gmr_out);
/* double fz_plot;
getGMRResult(gmr_perr_force, -gmr_in, fz_plot);*/
//-> INSTEAD OF SENDING GMR OUTPUT / SEND EST_EE_FT(Z)
// Send fz and distance to attractor for plotting
msg_ft.wrench.force.x = gmr_err;
msg_ft.wrench.force.y = gmr_out[0]; //ee_ft[2]
msg_ft.wrench.force.z = 0;
msg_ft.wrench.torque.x = 0;
msg_ft.wrench.torque.y = 0;
msg_ft.wrench.torque.z = 0;
pub_ee_ft_att_.publish(msg_ft);
// Hack! Scale the force to be in reasonable values
gmr_out[0] = FORCE_SCALING*fabs(gmr_out[0]);
ROS_INFO_STREAM_THROTTLE(0.5,"Distance to Attractor: " << new_err << " GMR output (N): " << gmr_out[0]);
gmr_msg.data = gmr_out[0];
pub_gmr_out_.publish(gmr_msg);
// Hack! Safety first!
if(gmr_out[0] > MAX_ROLLING_FORCE) {
gmr_out[0] = MAX_ROLLING_FORCE;
}
// Give some time for the force to catch up the first time. Then roll with constant force thereafter.
if(bfirst) {
sendAndWaitForNormalForce(-gmr_out[0]);
bfirst = false;
} else {
sendNormalForce(-gmr_out[0]);
}
ROS_INFO_STREAM_THROTTLE(0.5, "Force applied: "<<gmr_out[0]);
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Hack! Dont rely on model orientation. Use target orientation instead.
p.setRotation(trans_final_target.getRotation());
// Hack! Dont rely on the Z component of the model. It might go below the table!
p.getOrigin().setZ(trans_final_target.getOrigin().getZ());
homing=false;
break;
default:
ROS_ERROR_STREAM("No such phase defined "<<phase);
return false;
}
// Add rotation of Tool wrt. flange_link for BOXY
/*if (use_boxy_tool){
tf::Matrix3x3 R = p.getBasis();
Eigen::Matrix3d R_ee;
tf::matrixTFToEigen(R,R_ee);
Eigen::Matrix3d R_tool;
R_tool << -0.7071, -0.7071, 0.0,
0.7071,-0.7071, 0.0,
0.0, 0.0, 1.0;
//multiply tool rot
R_ee = R_ee*R_tool;
tf::matrixEigenToTF(R_ee,R);
p.setBasis(R);
}*/
// Send the computed pose from one of the above phases
sendPose(p);
// convert and send ee pose to attractor frame to plots
ee_pos_att.mult(trans_final_target.inverse(), p);
geometry_msgs::PoseStamped msg;
msg.pose.position.x = ee_pos_att.getOrigin().x();
msg.pose.position.y = ee_pos_att.getOrigin().y();
msg.pose.position.z = ee_pos_att.getOrigin().z();
msg.pose.orientation.x = ee_pos_att.getRotation().x();
msg.pose.orientation.y = ee_pos_att.getRotation().y();
msg.pose.orientation.z = ee_pos_att.getRotation().z();
msg.pose.orientation.w = ee_pos_att.getRotation().w();
pub_ee_pos_att_.publish(msg);
//ROS_INFO_STREAM_THROTTLE(0.5,"Error "<<prog_curr);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
sendPose(ee_pose);
sendNormalForce(0);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
feedback_.progress = prog_curr;
as_.publishFeedback(feedback_);
/* if(prog_curr < reachingThreshold) {
break;
}*/
//Orientation error 0.05
if(pos_err < reachingThreshold && (ori_err < 0.05 || isnan(ori_err))) {
break;
}
loop_rate.sleep();
}
delete cdsRun;
if(phase == PHASEREACH) {
// Hack! If phase is "reach", find the table right after reaching
if (bWaitForForces && !homing) {
bool x = find_table_for_rolling(0.35, 0.05, 5);
// bool x = find_table_for_rolling(0.35, 0.1, 5);
//-> Send command to dynamics plotter to stop logging
return x;
}
if (!homing){
//-> Send command to dynamics plotter to stop logging
}
} else if (phase == PHASEROLL){
// Hack! wait for zero force before getting ready to recieve further commands.
// This is to avoid dragging the dough.
sendAndWaitForNormalForce(0);
//-> Send command to dynamics plotter to start plotting
return ros::ok();
} else {
//->Send command to dynamics plotter to start plotting
return ros::ok();
}
}
/*!
* \brief Run the controller.
*
* The Controller generates desired velocities for every joints from the current positions.
*
*/
void run()
{
if(executing_)
{
watchdog_counter = 0;
if (as_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
{
/// set the action state to preempted
executing_ = false;
traj_generator_->isMoving = false;
ROS_INFO("Preempted trajectory action");
return;
}
std::vector<double> des_vel;
if(traj_generator_->step(q_current, des_vel))
{
if(!traj_generator_->isMoving) //Trajectory is finished
{
executing_ = false;
}
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(7);
for(unsigned int i=0; i<7; i++)
{
std::stringstream joint_name;
joint_name << "arm_" << (i+1) << "_joint";
target_joint_vel.velocities[i].joint_uri = joint_name.str();
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = des_vel.at(i);
}
/// send everything
joint_vel_pub_.publish(target_joint_vel);
}
else
{
ROS_INFO("An controller error occured!");
executing_ = false;
}
}
else
{
/// WATCHDOG TODO: don't always send
if(watchdog_counter < 10)
{
sensor_msgs::JointState target_joint_position;
target_joint_position.position.resize(7);
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(7);
for (unsigned int i = 0; i < 7; i += 1)
{
std::stringstream joint_name;
joint_name << "arm_" << (i+1) << "_joint";
target_joint_vel.velocities[i].joint_uri = joint_name.str();
target_joint_position.position[i] = 0;
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = 0;
}
joint_vel_pub_.publish(target_joint_vel);
joint_pos_pub_.publish(target_joint_position);
}
watchdog_counter++;
}
}
// run face recognition on the recieved image
void recognizeCb(const sensor_msgs::ImageConstPtr& msg) {
// cout << "entering.. recognizeCb" << endl;
_ph.getParam("algorithm", _recognitionAlgo);
if (_as.isPreemptRequested() || !ros::ok()) {
// std::cout << "preempt req or not ok" << std::endl;
ROS_INFO("%s: Preempted", _action_name.c_str());
// set the action state to preempted
_as.setPreempted();
// success = false;
// break;
// cout << "shutting down _image_sub" << endl;
_image_sub.shutdown();
Mat inactive(_window_rows, _window_cols, CV_8UC3, CV_RGB(20,20,20));
appendStatusBar(inactive, "INACTIVE", "");
cv::imshow(_windowName, inactive);
cv::waitKey(3);
return;
}
if (!_as.isActive()) {
// std::cout << "not active" << std::endl;
// cout << "shutting down _image_sub" << endl;
_image_sub.shutdown();
Mat inactive(_window_rows, _window_cols, CV_8UC3, CV_RGB(20,20,20));
appendStatusBar(inactive, "INACTIVE", "");
cv::imshow(_windowName, inactive);
cv::waitKey(3);
return;
}
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("%s: cv_bridge exception: %s", _action_name.c_str(), e.what());
return;
}
if (_window_rows == 0) {
_window_rows = cv_ptr->image.rows;
_window_cols = cv_ptr->image.cols;
}
// clear previous feedback
_feedback.names.clear();
_feedback.confidence.clear();
// _result.names.clear();
// _result.confidence.clear();
std::vector<cv::Rect> faces;
std::vector<cv::Mat> faceImgs = _fd.getFaceImgs(cv_ptr->image, faces, true);
std::map<string, std::pair<string, double> > results;
for( size_t i = 0; i < faceImgs.size(); i++ ) {
cv::resize(faceImgs[i], faceImgs[i], cv::Size(100.0, 100.0));
cv::cvtColor( faceImgs[i], faceImgs[i], CV_BGR2GRAY );
cv::equalizeHist( faceImgs[i], faceImgs[i] );
// perform recognition
results = _fr->recognize(faceImgs[i],
("eigenfaces" == _recognitionAlgo),
("fisherfaces" == _recognitionAlgo),
("lbph" == _recognitionAlgo)
);
ROS_INFO("Face %lu:", i);
if ("eigenfaces" == _recognitionAlgo)
ROS_INFO("\tEigenfaces: %s %f", results["eigenfaces"].first.c_str(), results["eigenfaces"].second);
if ("fisherfaces" == _recognitionAlgo)
ROS_INFO("\tFisherfaces: %s %f", results["fisherfaces"].first.c_str(), results["fisherfaces"].second);
if ("lbph" == _recognitionAlgo)
ROS_INFO("\tLBPH: %s %f", results["lbph"].first.c_str(), results["lbph"].second);
}
// update GUI window
// TODO check gui parameter
appendStatusBar(cv_ptr->image, "RECOGNITION", "");
cv::imshow(_windowName, cv_ptr->image);
cv::waitKey(3);
// if faces were detected
if (faceImgs.size() > 0) {
// recognize only once
if (_goal_id == 0) {
// std::cout << "goal_id 0. setting succeeded." << std::endl;
// cout << _recognitionAlgo << endl;
_result.names.push_back(results[_recognitionAlgo].first);
_result.confidence.push_back(results[_recognitionAlgo].second);
_as.setSucceeded(_result);
}
// recognize continuously
else {
_feedback.names.push_back(results[_recognitionAlgo].first);
_feedback.confidence.push_back(results[_recognitionAlgo].second);
_as.publishFeedback(_feedback);
}
}
}
void Trajectory_Tracker::track_trajectory(const planner::gen_trajGoalConstPtr &goal)
{
ROS_INFO("TRACKING TRAJECTORY");
float x = goal->x;
float y = goal->y;
x = x;
y = y;
if(goal->frame != "robot"){
geometry_msgs::Point goal_point = transform_goal_to_robot(x,y,goal->frame);
x = goal_point.x;
y = goal_point.y;
}
bool following_trajectory = true;
tf::StampedTransform frame_transform = broadcast_new_traj_frame();
current_trajectory.generate_trajectory(x,y);
cout << "generated trajectory" << endl;
ros::Rate loop_rate(10);
tf::StampedTransform transform;
tf::Quaternion q;
ros::Time start_time = ros::Time::now();
float t = 0;
while(t<=(current_trajectory.t_end)+1)
{
if (action_server.isPreemptRequested() || !ros::ok())
{
float u_v = 0;
float u_w = 0;
geometry_msgs::Pose2D cmd_vector;
cmd_vector.x = u_v;
cmd_vector.y = 0;
cmd_vector.theta = u_w;
cmd_pub.publish(cmd_vector);
result.success = 0;
action_server.setSucceeded(result);
cout << "CANCELING GOAL" << endl;
return;
}
t = (ros::Time::now()-start_time).toSec();
feedback.time = t;
action_server.publishFeedback(feedback);
Eigen::ArrayXf traj_at_t = current_trajectory.traj_time_lookup(t);
marker_pub.publish(current_trajectory.points);
marker_pub.publish(current_trajectory.line_strip);
// marker_pub.publish(current_trajectory.line_list);
transform.setOrigin( tf::Vector3(traj_at_t(1), traj_at_t(2), 0));
q.setRPY(0, 0, traj_at_t(3));
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/traj_frame", "/target"));
Eigen::ArrayXf error = get_error();
cout << error << endl;
// TODO: FEEDFORWARD
float u_v = traj_at_t(4);
float u_w = traj_at_t(5);
u_v += Kp_x*error(0);
u_w += Kp_y*error(1) + Kp_w*error(2);
geometry_msgs::Pose2D cmd_vector;
cmd_vector.x = u_v;
cmd_vector.y = 0;
cmd_vector.theta = u_w;
cmd_pub.publish(cmd_vector);
loop_rate.sleep();
if(!ros::ok())
{
result.success = 0;
action_server.setSucceeded(result);
return;
}
br.sendTransform(tf::StampedTransform(frame_transform, ros::Time::now(), "/world", "/traj_frame"));
}
geometry_msgs::Pose2D cmd_vector;
cmd_vector.x = 0;
cmd_vector.y = 0;
cmd_vector.theta = 0;
cmd_pub.publish(cmd_vector);
result.success = 1;
action_server.setSucceeded(result);
}
void executeCB(const corobot_face_recognition::FaceRecognitionGoalConstPtr &goal)
{
if (_as.isPreemptRequested() || !ros::ok()) {
ROS_INFO("%s: Preempted", _action_name.c_str());
// set the action state to preempted
_as.setPreempted();
// success = false;
// break;
}
// helper variables
ros::Rate r(60);
bool success = true;
_goal_id = goal->order_id;
_goal_argument = goal->order_argument;
_feedback.order_id = _goal_id;
_feedback.names.clear();
_feedback.confidence.clear();
_result.order_id = _goal_id;
_result.names.clear();
_result.confidence.clear();
// publish info to the console for the user
ROS_INFO("%s: Executing. order_id: %i, order_argument: %s ", _action_name.c_str(), _goal_id, _goal_argument.c_str());
switch (_goal_id) {
// RECOGNIZE ONCE
case 0:
// cout << "case 0" << endl;
// _fr = new FaceRec(true);
_image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::recognizeCb, this);
while( _as.isActive() && !_as.isPreemptRequested() && !ros::isShuttingDown() )
r.sleep();
break;
// RECOGNIZE CONTINUOUSLY
case 1:
//cout << "case 1" << endl;
// _fr = new FaceRec(true);
_image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::recognizeCb, this);
while( _as.isActive() && !_as.isPreemptRequested() && !ros::isShuttingDown() )
r.sleep();
break;
// ADD TRAINING IMAGES
case 2:
// cout << "case 2" << endl;
_fc = new FaceImgCapturer(_goal_argument);
_image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::addTrainingImagesCb, this);
while( _as.isActive() && !_as.isPreemptRequested() && !ros::isShuttingDown() )
r.sleep();
break;
// TRAIN DATABASE
case 3:
// call training function and check if successful
// bool trainingSuccessful = true;
// cout << "case 3" << endl;
_fr->train(_preprocessing);
if (true)
_as.setSucceeded(_result);
else
_as.setAborted(_result);
break;
// STOP
case 4:
if (_as.isActive()) {
_as.setPreempted();
// _image_sub.shutdown();
}
break;
// EXIT
case 5:
// cout << "case 4" << endl;
ROS_INFO("%s: Exit request.", _action_name.c_str());
_as.setSucceeded(_result);
r.sleep();
ros::shutdown();
break;
}
}
void executeCB(const hector_quadrotor_msgs::followerGoalConstPtr &goal)
{
ROS_INFO("Client is registered, lets start the executing");
publisher = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
gms_c = nh_.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
// smsl = m.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
getmodelstate.request.model_name="quadrotor";
geometry_msgs::Twist tw;
publisher.publish(tw);
ros::Duration(5.0).sleep();
gms_c.call(getmodelstate);
double now_x = getmodelstate.response.pose.position.x;
double now_y = getmodelstate.response.pose.position.y;
double now_z = getmodelstate.response.pose.position.z;
double new_x = goal->goal.position.x;
double new_y = goal->goal.position.y;
double new_z = goal->goal.position.z;
double x_diff, y_diff, z_diff;
double tmp_x, tmp_y, tmp_z;
double var_x, var_y, var_z;
ros::Rate r(1);
bool success = true;
// publish info to the console for the user
ROS_INFO("%s: Executing!", action_name_.c_str());
if (action_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
action_.setPreempted();
success = false;
//break;
}
publisher.publish(tw);
ros::Duration(2.0).sleep();
x_diff=new_x - now_x;
y_diff=new_y - now_y;
z_diff=new_z - now_z;
// if(now_z < 0)
// {
// now_z=now_z*(-1);
// z_diff=new_z - now_z;
// z_diff=z_diff*(-1);
// }else
// z_diff = new_z - now_z;
// tmp_x= modf(new_x, &var_x);
// tmp_y= modf(new_y, &var_y);
// tmp_z= modf(new_z, &var_z);
ROS_INFO(" Come Up Hector! ");
if(now_z < 0.5)
{
tw.linear.z = 0.6;
}
else
{
tw.linear.z = 0.0;
}
publisher.publish(tw);
ros::Duration(2.0).sleep();
// tw.angular.z = -0.5;
tw.linear.z = 0;
tw.linear.x = 0;
tw.linear.y = 0;
// publisher.publish(tw);
// ros::Duration(2.0).sleep();
// tw.angular.z = 0;
publisher.publish(tw);
ros::Duration(2.0).sleep();
ROS_INFO(" Good Boy, let's go further! ");
while( x_diff < -0.3 || x_diff > 0.3 || y_diff < -0.3 || y_diff > 0.3)
{
if(now_x < var_x || now_y < var_y)
{
tw.linear.x = x_diff * 0.1;
tw.linear.y = y_diff * 0.1;
}else
if(now_x > var_x || now_y > var_y)
{
tw.linear.x =x_diff * 0.1;
tw.linear.y =y_diff * 0.1;
}
publisher.publish(tw);
ros::Duration(2.0).sleep();
gms_c.call(getmodelstate);
now_y = getmodelstate.response.pose.position.y;
y_diff=new_y - now_y;
now_x = getmodelstate.response.pose.position.x;
x_diff=new_x - now_x;
}
ros::Duration(2.0).sleep();
tw.linear.z = 0;
tw.linear.x = 0;
tw.linear.y = 0;
// tw.angular.z = -0.5;
publisher.publish(tw);
ros::Duration(5.0).sleep();
// tw.angular.z = 0;
// publisher.publish(tw);
// ros::Duration(5.0).sleep();
action_.publishFeedback(feedback_);
// this sleep is not necessary, the sequence is computed at 1 }
if(success)
{
result_.result = feedback_.feedback;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
action_.setSucceeded(result_);
}}
void executeCB(const robot_actionlib::TestGoalConstPtr &goal)
{
// helper variables
ros::Rate loop_rate(25);
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]);
bool front = 0;
bool right = 0;
bool left = 0;
double right_average = 0;
double left_average = 0;
while(ros::ok())
{
ROS_INFO("Preempted ?: %d ", as_.isPreemptRequested());
ROS_INFO("Active ?: %d ", as_.isActive());
ROS_INFO("GOOALLLLLL: %d ", goal->order);
// 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;
}
front = (distance_front_left < front_limit) || (distance_front_right < front_limit);
right = (distance_rights_front < side_limit) && (distance_rights_back < side_limit);
left = (distance_lefts_front < side_limit) && (distance_lefts_back < side_limit);
right_average = (distance_rights_front + distance_rights_back)/2;
left_average = (distance_lefts_front + distance_lefts_back)/2;
flagReady = true;
if (!TestAction::flagReady) {
ros::spinOnce();
loop_rate.sleep();
continue;
}
// Update of the state
if(front){ // The front is the most prior
if(right_average > left_average){
side = 0;
}else{
side = 1;
}
state = FRONT;
std::cerr << "FRONT" << std::endl;
} else if (right) { // The right is prefered
/*if(state != RIGHT){
direction = rand()%2;
std::cerr << direction << std::endl;
}*/
state = RIGHT;
std::cerr << "RIGHT" << std::endl;
} else if (left) {
/*if(state != LEFT){
direction = rand()%2;
std::cerr << direction << std::endl;
}*/
//direction = rand()%1;
state = LEFT;
std::cerr << "LEFT" << std::endl;
} else {
state = EMPTY;
std::cerr << "EMPTY" << std::endl;
}
// Action depending on the state
switch(state) {
case(EMPTY): // Just go straight
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = smoothUpdateVelocity(msg.angular.z, 0, 0.1);
break;
case(FRONT):
msg.linear.x = 0;
if(side == 1){
msg.angular.z = smoothUpdateVelocity(msg.angular.z, a_speed, 0.1);
}else{
msg.angular.z = smoothUpdateVelocity(msg.angular.z, -a_speed, 0.1);
}
break;
case(RIGHT):
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = - alpha * ( (double)distance_rights_front - (double)distance_rights_back);
break;
case(LEFT):
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = alpha * ( (double)distance_lefts_front - (double)distance_lefts_back);
break;
default:
break;
}
// P-controller to generate the angular velocity of the robot, to follow the wall
//msg.angular.z = alpha * ( (double)distance_lefts_front- (double)distance_lefts_back);
//ROS_INFO("Linear velocity :%f", msg.linear.x);
//ROS_INFO("Angular velocity :%f", msg.angular.z);
if(counter > 10){
twist_pub.publish(msg);
}else {
counter += 1;
}
feedback_.sequence.push_back(feedback_.sequence[1] + feedback_.sequence[0]);
// publish the feedback
as_.publishFeedback(feedback_);
ros::spinOnce();
loop_rate.sleep();
}
if(success)
{
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
}
