void GoToSelectedBall::executeCB(const scheduler::SchedulerGoalConstPtr &goal){
ROS_INFO("enter executeCB, goal = %i", goal->value);
if(goal->value == 0){
state_ = STOP;
}
else if(goal->value == 1){
state_ = FIRST_STEP_COLLECT;
}
else if(goal->value == 2){
// TODO: sprawdza, czy jest ustawiona pozycja pileczki, albo przesylac ja razem z goalem
// TODO: dopisac serwer do jazdy do przodu a nie na sleep tak jak teraz
state_ = SECOND_STEP_COLLECT;
goForward(0);
ROS_INFO("enter SECOND_STEP_COLLECT");
publishAngle();
ac.waitForResult();
float dist = getDistanceFromSelectedBall();
onHoover();
goForward(dist -0.3);
ros::Duration(4.0).sleep();
goForward(-(dist-0.3));
ros::Duration(5.0).sleep();
goForward(0);
offHoover();
ROS_INFO("leave SECOND_STEP_COLLECT");
}
as_.publishFeedback(feedback_);
result_.value = feedback_.value;
as_.setSucceeded(result_);
ROS_INFO("leave executeCB");
}
void 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 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();
}
}
//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 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_);
}
}
// Go down until hit the table. For safety min_height is specified. If no table found until this height, returns false.
// vertical_speed with which to move downwards
// thr_force - normal force threshold at which table is assumed to be detected
bool find_table_for_rolling(double min_height, double vertical_speed, double thr_force) {
double rate = 200;
thr_force = fabs(thr_force);
ros::Rate thread_rate(rate);
double startz = ee_pose.getOrigin().z();
msg_pose.pose.position.x = ee_pose.getOrigin().x();
msg_pose.pose.position.y = ee_pose.getOrigin().y();
msg_pose.pose.position.z = startz;
msg_pose.pose.orientation.x = ee_pose.getRotation().x();
msg_pose.pose.orientation.y = ee_pose.getRotation().y();
msg_pose.pose.orientation.z = ee_pose.getRotation().z();
msg_pose.pose.orientation.w = ee_pose.getRotation().w();
// Publish attractors if running in simulation or with fixed values
if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
static tf::TransformBroadcaster br;
tf::Transform table;
table.setOrigin(tf::Vector3 (ee_pose.getOrigin().x(),ee_pose.getOrigin().y(),ee_pose.getOrigin().z() - min_height));
table.setRotation(tf::Quaternion (ee_pose.getRotation().x(),ee_pose.getRotation().y(),ee_pose.getRotation().z(),ee_pose.getRotation().w()));
br.sendTransform(tf::StampedTransform(table, ros::Time::now(), world_frame, "/attractor"));
}
ROS_INFO_STREAM("Finding table up to max dist. "<<min_height<<" with vertical speed "<<vertical_speed<<" and threshold force "<<thr_force<<"N.");
while(ros::ok()) {
msg_pose.pose.position.z = msg_pose.pose.position.z - vertical_speed/rate;
pub_.publish(msg_pose);
// Go down until force reaches the threshold
if(fabs(ee_ft[2]) > thr_force) {
break;
}
if(fabs(ee_pose.getOrigin().z()-startz) > min_height) {
ROS_INFO("Max distance reached");
return false;
}
thread_rate.sleep();
feedback_.progress = ee_ft[2];
as_.publishFeedback(feedback_);
}
if(!ros::ok()) {
return false;
}
tf::Vector3 table(ee_pose.getOrigin());
ROS_INFO_STREAM("Table found at height "<<table[2]);
msg_pose.pose.position.z = table[2];
pub_.publish(msg_pose);
sendAndWaitForNormalForce(0);
return true;
}
bool polling( const std::vector<double> &j1 ) {
std::vector<double> j2;
j2.resize(6);
if(staubli.GetRobotJoints(j2)){
feedback_.j = j2;
as_.publishFeedback(feedback_);
double error = fabs(j1[0]-j2[0])+ fabs(j1[1]-j2[1])+ fabs(j1[2]-j2[2])+
fabs(j1[3]-j2[3])+ fabs(j1[4]-j2[4])+ fabs(j1[5]-j2[5]);
// ROS_INFO( "Error to target %lf", error );
return error < ERROR_EPSILON || staubli.IsJointQueueEmpty();
}else {
ROS_ERROR("Error when determining end of movement.");
return false;
}
}
//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 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_);
}
}
// 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();
}
// Roll with "force" and horizontal "speed" until the length "range"
bool rolling(double force, double speed, double range) {
ROS_INFO_STREAM("Rolling with force "<<force<<", speed "<<speed<<", range "<<range);
force = fabs(force);
sendNormalForce(-force);
msg_pose.pose.position.x = ee_pose.getOrigin().x();
msg_pose.pose.position.y = ee_pose.getOrigin().y();
msg_pose.pose.position.z = ee_pose.getOrigin().z();
tf::Quaternion q = ee_pose.getRotation();
msg_pose.pose.orientation.x = q.x();
msg_pose.pose.orientation.y = q.y();
msg_pose.pose.orientation.z = q.z();
msg_pose.pose.orientation.w = q.w();
double center = ee_pose.getOrigin().y();
double rate = 200;
ros::Rate thread_rate(rate);
int count=0;
while(ros::ok()) {
msg_pose.pose.position.y = msg_pose.pose.position.y + speed/rate;
feedback_.progress = msg_pose.pose.position.y;
as_.publishFeedback(feedback_);
if( fabs(msg_pose.pose.position.y - center) > range) {
ROS_INFO("Change direction");
speed *= -1;
if(++count > 5) {
break;
}
}
pub_.publish(msg_pose);
thread_rate.sleep();
}
msg_pose.pose.position.z = ee_pose.getOrigin().z() + 0.15;
pub_.publish(msg_pose);
sendNormalForce(0);
return true;
}
void jointStateCallback(const sensor_msgs::JointState::ConstPtr &msg) {
size_t msgSize = msg->name.size(), jointInfoSize = _jointInfo.name.size(), msgSizeP = msg->position.size(),
msgSizeV = msg->velocity.size(), msgSizeE = msg->effort.size();
for (int i = 0; i < msgSize; i++) {
for (int j = 0; j < jointInfoSize; j++) {
if (msg->name[i] == _jointInfo.name[j]) {
if (i < msgSizeP) _jointInfo.position[j] = msg->position[i];
if (i < msgSizeV) _jointInfo.velocity[j] = msg->velocity[i];
if (i < msgSizeE) _jointInfo.effort[j] = msg->effort[i];
}
}
}
if(_publishFeedback) {
size_t size = _feedback.joint_names.size();
_feedback.actual.positions.clear();
_feedback.actual.velocities.clear();
_feedback.actual.effort.clear();
_feedback.error.positions.clear();
_feedback.error.velocities.clear();
_feedback.error.effort.clear();
for (int i = 0; i < size; ++i) {
feedback_t singleJointFeedback = getFeedback(_feedback.joint_names[i], _feedback.desired.positions[i],
0.0,
_feedback.desired.velocities[i]);
_feedback.actual.positions.push_back(singleJointFeedback.actual.position);
_feedback.actual.velocities.push_back(singleJointFeedback.actual.velocity);
_feedback.actual.effort.push_back(singleJointFeedback.actual.effort);
_feedback.error.positions.push_back(singleJointFeedback.error.position);
_feedback.error.velocities.push_back(singleJointFeedback.error.velocity);
_feedback.error.effort.push_back(singleJointFeedback.error.effort);
}
_actionServer.publishFeedback(_feedback);
}
}
// returns the status to the client (Behavior Tree)
void set_status(int status)
{
// Set The feedback and result of BT.action
feedback_.status = status;
result_.status = feedback_.status;
// publish the feedback
as_.publishFeedback(feedback_);
// setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE).
as_.setSucceeded(result_);
switch (status) // Print for convenience
{
case SUCCESS:
ROS_INFO("Action %s Succeeded", ros::this_node::getName().c_str() );
break;
case FAILURE:
ROS_INFO("Action %s Failed", ros::this_node::getName().c_str() );
break;
default:
break;
}
}
// metoda jest uruchamiana w osobnym watku i nie blokuje calbackow
void Explore::executeCB(const scheduler::SchedulerGoalConstPtr &goal){
ROS_INFO("enter executeCB, goal = %i", goal->value);
if(goal->value == 1){
explore_ = true;
}
else if(goal->value == 0){
// koniec ekslporacji
stopExplore();
explore_state_ = STOP;
explore_ = false;
}
feedback_.value = 0;
as_.publishFeedback(feedback_);
result_.value = feedback_.value;
as_.setSucceeded(result_);
ROS_INFO("leave executeCB");
}
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 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
}
/**
* @brief Feedback callback for the move_base client, republishes as feedback for the exploration server
* @param feedback Feedback from the move_base client
*/
void feedbackMovingCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback){
feedback_.base_position = feedback->base_position;
as_.publishFeedback(feedback_);
}
void Update(const ros::TimerEvent& e) {
if(!server_.isActive() || is_running)
return;
if(got_goals)
{
is_running = true;
ROS_INFO("Update--> Got goals of size %d", num_feature_goals_);
feedback_.features.resize(num_feature_goals_);
feedback_.signal.resize(num_feature_goals_);
// feedback_.signal.resize(num_feature_goals_);
for(int i=0; i<num_feature_goals_; i++)
{
feedback_.signal[i].signal_type = "hue";
feedback_.signal[i].mean.resize(1);
feedback_.signal[i].variance.resize(1);
feedback_.signal[i].mean[0] = feature_mean_goals_[i];
feedback_.signal[i].variance[0] = feature_var_goals_[i];
feedback_.signal[i].size = 1;
mean_color = feature_mean_goals_[i];
window_size = feature_var_goals_[i];
int rangeMin = ((mean_color - window_size + 255)%255);
int rangeMax = ((mean_color + window_size + 255)%255);
ROS_INFO("Range [%d] : %d,%d",i,rangeMin,rangeMax);
ROS_INFO("Threshold : %d,%d",minCCThreshold,maxCCThreshold);
if(rangeMin > rangeMax){
int temp = rangeMax;
rangeMax = rangeMin;
rangeMin = temp;
}
if(minCCThreshold >= maxCCThreshold){
//ROS_INFO("Min radius must be smaller than Max radius");
is_running = false;
return;
}
if(fabs(rangeMin - rangeMax) <= 2*window_size){
//ROS_INFO("REG rangeMin %d rangeMax %d", rangeMin, rangeMax);
inRange(*hue_image, Scalar((double)((uchar)rangeMin)),Scalar((double)((uchar)rangeMax)), *back_img);
}
else if ((mean_color + window_size) > 255){
//ROS_INFO("BIG rangeMin %d rangeMax %d", rangeMin, rangeMax);
inRange(*hue_image, Scalar((double)((uchar)rangeMax)),Scalar((double)((uchar)255)), *back_img);
}
else {
//ROS_INFO("SML rangeMin %d rangeMax %d", rangeMin, rangeMax);
inRange(*hue_image, Scalar((double)((uchar)0)),Scalar((double)((uchar)rangeMin)), *back_img);
}
Size ksize = Size(2 * blurKernel + 1,2 * blurKernel + 1);
ROS_INFO("Adding Gaussian Blur");
GaussianBlur(*back_img, *blurred_image, ksize, -1, -1);
ROS_INFO("Thresholding --->");
threshold(*blurred_image, *temp_blurred_image, 110, 255, THRESH_BINARY);
convertScaleAbs(*temp_blurred_image, *back_img, 1, 0);
//Find Connected Components
ROS_INFO("Finding Connected Comps for feature %d",i);
getConnectedComponents(i);
ROS_INFO("After finding conn comps, num[%d] : %d",i,numCC[i]);
feedback_.features[i].num = numCC[i];
if(numCC[i] > 0)
feedback_.features[i].moments.resize(numCC[i]);
ROS_INFO("Getting Moments");
if(numCC[i] > 0)
getMoments(i);
ROS_INFO("Updating KF ");
blobTracker[i].updateKalmanFiltersConnectedComponents();
if (numCC[i] > 0)
blobTracker[i].getFilteredBlobs(true);
else
blobTracker[i].getFilteredBlobs(false);
RotatedRect box;
Point pt;
for(int j=0; j<maxNumComponents; j++)
{
FeatureBlob ftemp;
blobTracker[i].filters_[j].getFiltered(ftemp);
if(ftemp.getValid() && display_image)
{
Mat firstTemp, secondTemp;
ftemp.getValues(firstTemp, secondTemp);
pt.x = firstTemp.at<float>(0,0); pt.y = firstTemp.at<float>(1,0);
blobTracker[i].getBoxFromCov(pt, secondTemp, box);
if (box.size.width > 0 && box.size.height > 0 && box.size.width < width && box.size.height < height)
{
ellipse(*rgb_image, box, CV_RGB(255,255,255), 3, 8);
circle(*rgb_image, pt, 3, CV_RGB(255, 255, 255), -1, 8);
}
}
}
}
if (display_image) {
imshow(name, *rgb_image);
}
server_.publishFeedback(feedback_);
is_running = false;
waitKey(100);
}
}
// 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();
}
}
void executeCB(const race_obstacle_detector::WaitUntilUnblockedGoalConstPtr &goal)
{
std::string obstacle_class = "unknown"; // means also there is NO obstacle
// publish info to the console for the user
ROS_INFO("%s: Executing, with pose %4.2f,%4.2f,%4.2f",
action_name_.c_str(),
goal->boundingbox.pose_stamped.pose.position.x,
goal->boundingbox.pose_stamped.pose.position.y,
goal->boundingbox.pose_stamped.pose.position.z
);
//Point the head
pointHead(goal->boundingbox, 1.3);
//add timeout
ros::Duration duration;
if ( goal->timeout <= ros::Duration(0.0) )
duration = ros::Duration(1000000.0); // wait "forever"
else
duration = goal->timeout;
ros::Time exp_time = ros::Time::now() + duration;
ROS_INFO("timeout duration: %4.2f, at time: %4.2f", duration.toSec(), exp_time.toSec());
// Action takes until it is canceled, timeout occured or no obstacle has
// been detected
// TODO average obstacle check over time
while (true)
{
// 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();
return;
}
// check if the timeout has occured
if (exp_time <= ros::Time::now())
{
ROS_INFO("Timeout hit at %4.2f", ros::Time::now().toSec());
ROS_INFO("%s: Aborted", action_name_.c_str());
as_.setAborted();
return;
}
//==========Obstacle Decisions here===============
obstacle_mutex.lock();
feedback_.obstacle = obstacle_detected;
if (!obstacle_detected)
obstacle_class = "unknown";
else if (human_detected)
obstacle_class = "human";
else
obstacle_class = "table";
obstacle_mutex.unlock();
//================================================
feedback_.obstacle_type = obstacle_class ;
feedback_.confidence = 0.0; // dummy value
feedback_.boundingbox.pose_stamped.header.frame_id="/base_link";
feedback_.boundingbox.pose_stamped.header.stamp=ros::Time::now();
//ROS_INFO("before lock");
//mutex lock cloud_bb
bb_mutex.lock();
feedback_.boundingbox.pose_stamped.pose.position.x=(cloud_bb.x_max+cloud_bb.x_min)/2;
feedback_.boundingbox.pose_stamped.pose.position.y=(cloud_bb.y_max+cloud_bb.y_min)/2;
feedback_.boundingbox.pose_stamped.pose.position.z=(cloud_bb.z_max+cloud_bb.z_min)/2;
feedback_.boundingbox.pose_stamped.pose.orientation.x=0;
feedback_.boundingbox.pose_stamped.pose.orientation.y=0;
feedback_.boundingbox.pose_stamped.pose.orientation.z=0;
feedback_.boundingbox.pose_stamped.pose.orientation.w=1;
feedback_.boundingbox.dimensions.x=cloud_bb.x_max-cloud_bb.x_min;
feedback_.boundingbox.dimensions.y=cloud_bb.y_max-cloud_bb.y_min;
feedback_.boundingbox.dimensions.z=cloud_bb.z_max-cloud_bb.z_min;
bb_mutex.unlock();
//obstacle = feedback_.obstacle;
ROS_INFO("%s: Feedback publishing, obstacle: %s", action_name_.c_str(), obstacle_class .c_str());
// publish feedback here
as_.publishFeedback(feedback_);
//finally set action to succeeded
if (false == obstacle_detected)
{
as_.setSucceeded(result_);
ROS_INFO("%s: Succeeded..", action_name_.c_str());
return;
}
ros::Duration(1.0).sleep();
}
}
void executeCB(const vizzy_expressions::ExpressionGoalConstPtr &goal)
{
bool success = true;
feedback_.state_reached = false;
if(goal->mode == goal->PREDEFINED)
{
ROS_INFO("Changing emotion for part %s to %s", goal->subsystem.c_str(), goal->emotion.c_str());
if(goal->subsystem == goal->PART_ALL)
{
bool sentM(false), sentS(false), sentR(false), sentL(false);
//If the goal is a valid emotion
if(goal->emotion == goal->FACE_ANGRY || goal->emotion == goal->FACE_CUNNING || goal->emotion == goal->FACE_EVIL
|| goal->emotion == goal->FACE_HAPPY || goal->emotion == goal->FACE_NEUTRAL || goal->emotion == goal->FACE_SAD
|| goal->emotion == goal->FACE_SHY || goal->emotion == goal->FACE_SURPRISED
)
{
sentS = driverComms->sendCommand(ExpressionValues::ExpressionValue(goal->emotion, goal->PART_EYELIDS), goal->PART_EYELIDS);
if(sentS)
{
feedback_.eyelids_value = ExpressionValues::ExpressionValue(goal->emotion, goal->PART_EYELIDS);
feedback_.eyelids_emotion = goal->emotion;
}
as_.publishFeedback(feedback_);
sentL = driverComms->sendCommand(ExpressionValues::ExpressionValue(goal->emotion, goal->PART_LEFTEYEBROW), goal->PART_LEFTEYEBROW);
if(sentL)
{
feedback_.leftEyebrow_value = ExpressionValues::ExpressionValue(goal->emotion, goal->PART_LEFTEYEBROW);
feedback_.leftEyebrow_emotion = goal->emotion;
}
as_.publishFeedback(feedback_);
sentM = driverComms->sendCommand(ExpressionValues::ExpressionValue(goal->emotion, goal->PART_MOUTH), goal->PART_MOUTH);
if(sentM)
{
feedback_.mouth_value = ExpressionValues::ExpressionValue(goal->emotion, goal->PART_MOUTH);
feedback_.mouth_emotion = goal->emotion;
}
as_.publishFeedback(feedback_);
sentR = driverComms->sendCommand(ExpressionValues::ExpressionValue(goal->emotion, goal->PART_RIGHTEYEBROW), goal->PART_RIGHTEYEBROW);
if(sentR)
{
feedback_.rightEyebrow_value = ExpressionValues::ExpressionValue(goal->emotion, goal->PART_RIGHTEYEBROW);
feedback_.rightEyebrow_emotion = goal->emotion;
}
as_.publishFeedback(feedback_);
}else
{
ROS_ERROR("Invalid expression...");
success = false;
}
success = sentS && sentL && sentM && sentR;
feedback_.state_reached = success;
as_.publishFeedback(feedback_);
}else if(goal->subsystem == goal->PART_EYELIDS || goal->subsystem == goal->PART_LEFTEYEBROW || goal->subsystem == goal->PART_MOUTH || goal->subsystem == goal->PART_RIGHTEYEBROW)
{
success = driverComms->sendCommand(ExpressionValues::ExpressionValue(goal->emotion, goal->subsystem), goal->subsystem);
if(success)
{
if(goal->subsystem == goal->PART_EYELIDS)
{
feedback_.eyelids_value = ExpressionValues::ExpressionValue(goal->emotion, goal->PART_EYELIDS);
feedback_.eyelids_emotion = goal->emotion;
}
else if(goal->subsystem == goal->PART_LEFTEYEBROW)
{
feedback_.leftEyebrow_value = ExpressionValues::ExpressionValue(goal->emotion, goal->PART_LEFTEYEBROW);
feedback_.leftEyebrow_emotion = goal->emotion;
}else if(goal->subsystem == goal->PART_MOUTH)
{
feedback_.mouth_value = ExpressionValues::ExpressionValue(goal->emotion, goal->PART_MOUTH);
feedback_.mouth_emotion = goal->emotion;
}else if(goal->subsystem == goal->PART_RIGHTEYEBROW)
{
feedback_.rightEyebrow_value = ExpressionValues::ExpressionValue(goal->emotion, goal->PART_RIGHTEYEBROW);
feedback_.rightEyebrow_emotion = goal->emotion;
}
}
feedback_.state_reached = success;
as_.publishFeedback(feedback_);
}
else
{
ROS_ERROR("Invalid part...");
success = false;
}
result_.eyelids_emotion = feedback_.eyelids_emotion;
result_.leftEyebrow_emotion = feedback_.leftEyebrow_emotion;
result_.rightEyebrow_emotion = feedback_.rightEyebrow_emotion;
result_.mouth_emotion = feedback_.mouth_emotion;
result_.eyelids_value = feedback_.eyelids_value;
result_.leftEyebrow_value = feedback_.leftEyebrow_value;
result_.rightEyebrow_value = feedback_.rightEyebrow_value;
result_.mouth_value = feedback_.mouth_value;
result_.state_reached = success;
if(success)
{
ROS_INFO("Emotion goal reached");
as_.setSucceeded(result_);
}
else
{
ROS_INFO("Emotion goal failed...");
as_.setAborted(result_);
}
}else if(goal->mode == goal-> LOWLEVEL)
{
std::stringstream m;
std::stringstream s;
std::stringstream r;
std::stringstream l;
m << std::hex << std::uppercase << +goal->mouth_value;
s << std::hex << std::uppercase << +goal->eyelids_value;
r << std::hex << std::uppercase << +goal->rightEyebrow_value;
l << std::hex << std::uppercase << +goal->leftEyebrow_value;
ROS_INFO("Changing to non predefined emotion. Mouth: %s, Eyelids: %s, RightEyebrow: %s, LeftEyebrow: %s", m.str().c_str(), s.str().c_str(), r.str().c_str(), l.str().c_str());
bool sentM(false), sentS(false), sentR(false), sentL(false);
sentM = driverComms->sendCommand(goal->mouth_value, goal->PART_MOUTH);
sentS = driverComms->sendCommand(goal->eyelids_value, goal->PART_EYELIDS);
sentR = driverComms->sendCommand(goal->rightEyebrow_value, goal->PART_RIGHTEYEBROW);
sentL = driverComms->sendCommand(goal->leftEyebrow_value, goal->PART_LEFTEYEBROW);
success = sentM && sentS && sentR && sentL;
result_.eyelids_emotion = feedback_.eyelids_emotion = feedback_.FACE_UNDEFINED;
result_.leftEyebrow_emotion = feedback_.leftEyebrow_emotion = feedback_.FACE_UNDEFINED;
result_.rightEyebrow_emotion = feedback_.rightEyebrow_emotion = feedback_.FACE_UNDEFINED;
result_.mouth_emotion = feedback_.mouth_emotion = feedback_.FACE_UNDEFINED;
result_.eyelids_value = feedback_.eyelids_value;
result_.leftEyebrow_value = feedback_.leftEyebrow_value;
result_.rightEyebrow_value = feedback_.rightEyebrow_value;
result_.mouth_value = feedback_.mouth_value;
result_.state_reached = success;
if(success)
{
ROS_INFO("Lowlevel emotion goal reached");
as_.setSucceeded(result_);
}
else{
ROS_INFO("Lowlevel emotion goal failed...");
as_.setAborted(result_);
}
}
else
{
ROS_ERROR("Invalid emotion mode...");
result_.state_reached = false;
as_.setAborted(result_);
}
}
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;
}
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 trackPointCB(const geometry_msgs::Point::ConstPtr& msg)
{
// make sure that the action hasn't been canceled
if (!as_.isActive())
{
twist_.angular.z = 0;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
return;
}
if( !has_goal_)
{
twist_.angular.z = 0;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
return;
}
if( obstacle_detected_ && msg->y < SVTIUS)
{
result_.success = false;
ROS_INFO("%s: Obsticle Detected", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
feedback_.success = false;
twist_.angular.z = 0;
twist_.linear.x = 0;
if ( msg->y >= TOP_T
&& msg->x <= RIGHT_T
&& msg->x >= LEFT_T )
{
twist_pub_.publish(twist_);
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
result_.success = true;
as_.setSucceeded(result_);
}
if(drop_flag) {
twist_pub_.publish(twist_);
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
result_.success = true;
drop_flag = false;
as_.setSucceeded(result_);
}
if (msg->y < TOP_T)
{
ROS_INFO("MOVE FORWARD");
twist_.linear.x = getLinVel(msg->y, TOP_T);
}
if (msg->x < LEFT_T)
{
ROS_INFO("TURN RIGHT");
twist_.angular.z = getAngVel(msg->x, LEFT_T);
}
else if (msg->x > RIGHT_T)
{
ROS_INFO("TURN LEFT");
twist_.angular.z = -1.0 * getAngVel(msg->x, RIGHT_T);
}
twist_pub_.publish(twist_);
as_.publishFeedback(feedback_);
}
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_);
}}
bool turnOdom(double radians)
{
// If the distance to travel is negligble, don't even try.
if (fabs(radians) < 0.01)
return true;
while(radians < -M_PI) radians += 2*M_PI;
while(radians > M_PI) radians -= 2*M_PI;
//we will record transforms here
tf::StampedTransform start_transform;
tf::StampedTransform current_transform;
try
{
//wait for the listener to get the first message
listener_.waitForTransform(base_frame, odom_frame,
ros::Time::now(), ros::Duration(1.0));
//record the starting transform from the odometry to the base frame
listener_.lookupTransform(base_frame, odom_frame,
ros::Time(0), start_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
return false;
}
//we will be sending commands of type "twist"
geometry_msgs::Twist base_cmd;
//the command will be to turn at 0.75 rad/s
base_cmd.linear.x = base_cmd.linear.y = 0.0;
base_cmd.angular.z = turn_rate;
if (radians < 0)
base_cmd.angular.z = -turn_rate;
//the axis we want to be rotating by
tf::Vector3 desired_turn_axis(0,0,1);
ros::Rate rate(25.0);
bool done = false;
while (!done && nh_.ok() && as_.isActive())
{
//send the drive command
cmd_vel_pub_.publish(base_cmd);
rate.sleep();
//get the current transform
try
{
listener_.lookupTransform(base_frame, odom_frame,
ros::Time(0), current_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
break;
}
tf::Transform relative_transform =
start_transform.inverse() * current_transform;
tf::Vector3 actual_turn_axis =
relative_transform.getRotation().getAxis();
double angle_turned = relative_transform.getRotation().getAngle();
// Update feedback and result.
feedback_.turn_distance = angle_turned;
result_.turn_distance = angle_turned;
as_.publishFeedback(feedback_);
if ( fabs(angle_turned) < 1.0e-2) continue;
//if ( actual_turn_axis.dot( desired_turn_axis ) < 0 )
// angle_turned = 2 * M_PI - angle_turned;
if (fabs(angle_turned) > fabs(radians)) done = true;
}
if (done) return true;
return false;
}
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_);
}
}
// 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);
}
}
}
bool driveForwardOdom(double distance)
{
// If the distance to travel is negligble, don't even try.
if (fabs(distance) < 0.01)
return true;
//we will record transforms here
tf::StampedTransform start_transform;
tf::StampedTransform current_transform;
try
{
//wait for the listener to get the first message
listener_.waitForTransform(base_frame, odom_frame,
ros::Time::now(), ros::Duration(1.0));
//record the starting transform from the odometry to the base frame
listener_.lookupTransform(base_frame, odom_frame,
ros::Time(0), start_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
return false;
}
//we will be sending commands of type "twist"
geometry_msgs::Twist base_cmd;
//the command will be to go forward at 0.25 m/s
base_cmd.linear.y = base_cmd.angular.z = 0;
base_cmd.linear.x = forward_rate;
if (distance < 0)
base_cmd.linear.x = -base_cmd.linear.x;
ros::Rate rate(25.0);
bool done = false;
while (!done && nh_.ok() && as_.isActive())
{
//send the drive command
cmd_vel_pub_.publish(base_cmd);
rate.sleep();
//get the current transform
try
{
listener_.lookupTransform(base_frame, odom_frame,
ros::Time(0), current_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
break;
}
//see how far we've traveled
tf::Transform relative_transform =
start_transform.inverse() * current_transform;
double dist_moved = relative_transform.getOrigin().length();
// Update feedback and result.
feedback_.forward_distance = dist_moved;
result_.forward_distance = dist_moved;
as_.publishFeedback(feedback_);
if(fabs(dist_moved) > fabs(distance))
{
done = true;
}
}
base_cmd.linear.x = 0.0;
base_cmd.angular.z = 0.0;
cmd_vel_pub_.publish(base_cmd);
if (done) return true;
return false;
}
