int executeCB(ros::Duration dt)
{
std::cout << "**GoToPoint -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**GoToPoint -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
if(!init_)
{
set_feedback(RUNNING);
initialize();
}
if(sqrt((r.x-p.x)*(r.x-p.x) + (r.y-p.y)*(r.y-p.y)) > distThreshold)
{
geometry_msgs::Twist cmd = controller();
cmd_pub.publish(cmd);
}
else
{
set_feedback(SUCCESS);
finalize();
return 1;
}
//ROS_INFO("x = %f, y = %f, theta = %f",r.x,r.y,r.theta);
return 0;
}
int executeCB(ros::Duration dt)
{
std::cout << "**Battery Level -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**Battery Level -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
if (!init_)
{
initialize();
init_ = true;
battery_level = 0;
// battery_proxy_ptr.enablePowerMonitoring(true);
}
battery_level = battery_proxy_ptr->getBatteryCharge();
std::cout <<"The battery is at: " << battery_level<< "%"<< std::endl;
set_feedback(RUNNING);
if(battery_level<5){
set_feedback(SUCCESS);
std::cout <<"The battery level is low " << std::endl;
return 0;
}
std::cout <<"The battery level is OK " << std::endl;
set_feedback(FAILURE);
return 1;
}
int executeCB(ros::Duration dt)
{
std::cout << "**Search -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**Search -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
if(!init_)
{
set_feedback(RUNNING);
initialize();
// Launch Particle Filter
ic = new ImageConverter();
// Start rotating
geometry_msgs::Twist cmd;
cmd.angular.z = 0.5;
cmd_pub.publish(cmd);
}
if(robotDetected)
{
set_feedback(SUCCESS);
finalize();
return 1;
}
return 0;
}
// the action succeeds automatically after 5 seconds
int executeCB(ros::Duration dt)
{
std::cout << "**ActionName -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**ActionName -%- elapsed_time: "
<< time_to_complete_.toSec() << std::endl;
time_to_complete_ += dt;
if (time_to_complete_.toSec() < 5)
{
set_feedback(RUNNING);
// feedback_.FEEDBACK_ = RUNNING;
// as_.publishFeedback(feedback_);
return 0; // 'allows' this thread to continue running
}
else if (time_to_complete_.toSec() >= 5)
{
set_feedback(SUCCESS);
// set_feedback(FAILURE);
// feedback_.FEEDBACK_ = FAILURE;
// as_.publishFeedback(feedback_);
// result_.RESULT_ = FAILURE;
// as_.setSucceeded(result_);
// stop(); // stop allows new threads to be created
return 1; // 'forces' this thread to finish securely
}
return 0;
}
int executeCB(ros::Duration dt)
{
std::cout << "**Is Activity Stand-%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**Is Activity Stand-%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
if (!init_)
{
//initialize();
init_ = true;
last_activity_name = "none";
}
//set_feedback(RUNNING);
std::cout <<"Is Dance?: " << last_activity_name << std::endl;
if(last_activity_name.compare("Dance") == 0){
set_feedback(SUCCESS);
finalize();
return 1;
}else{
set_feedback(FAILURE);
behavior_proxy_ptr->stopBehavior("macarena");
finalize();
return 1;
}
}
int Condition::executeCB(ros::Duration dt)
{
std::string object_name, ids_arm, ids_bin;
bool has_memory;
set_feedback(RUNNING);
if (!isSystemActive())
{
return 1;
}
fillParameter("/apc/task_manager/target_object", object_name);
fillParameter("/apc/task_manager/memory/" + object_name, false, has_memory);
if (has_memory)
{
fillParameter("/apc/task_manager/memory/ids_arm", ids_arm);
fillParameter("/apc/task_manager/memory/ids_bin", ids_bin);
setParameter("/apc/bt/ids", ids_arm);
setParameter("/apc/bt/ids_bin", ids_bin);
setParameter("/apc/task_manager/memory/" + object_name, false);
set_feedback(SUCCESS);
return 1;
}
set_feedback(FAILURE);
return 1;
}
int SetIDSAction::executeCB(ros::Duration dt)
{
set_feedback(RUNNING);
std::string bin, ids_bin, old_arm, arm, task;
fillParameter("/apc/task", task);
if (task != "stow")
{
if (!fillParameter("/apc/task_manager/target_bin", bin))
{
set_feedback(FAILURE);
return 1;
}
}
else
{
bin = "tote";
}
ids_bin = bin + "_1";
arm = left_arm;
fillParameter("/apc/bt/ids", arm, old_arm); // get old ids arm, or use the current one, if not set
setParameter("/apc/bt/old_ids", old_arm);
setParameter("/apc/bt/ids", arm);
setParameter("/apc/bt/ids_bin", ids_bin);
set_feedback(SUCCESS);
return 1;
}
int SetIDSAction::executeCB(ros::Duration dt)
{
set_feedback(RUNNING);
setParameter("/apc/task_manager/memory/", true);
set_feedback(SUCCESS);
return 1;
}
int Condition::executeCB(ros::Duration dt)
{
set_feedback(RUNNING);
if (!isSystemActive())
{
return 1;
}
// INSERT CODE TO CHECK IF OBJECT IS SIMTRACKABLE
set_feedback(SUCCESS);
return 1;
}
int executeCB(ros::Duration dt)
{
std::cout << "**GoClose -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**GoClose -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
if(!init_)
{
set_feedback(RUNNING);
initialize();
// Launch Particle Filter
ic = new ImageConverter();
}
// Robot not detected
if(!robotDetected)
{
set_feedback(FAILURE);
finalize();
return 1;
}
// Close to the other robot
//ROS_INFO("Depth = %f, r = %f, l = %f",depth,right,left);
if(((right < dist_threshold) | (left < dist_threshold)) & (depth < dist_threshold))
{
set_feedback(SUCCESS);
finalize();
return 1;
}
// Controller
int y_rel = y - width/2;
double angular = -alpha*y_rel;
// Thresholds
if(angular > 1) {angular = 1;}
if(angular < -1) {angular = -1;}
// Publish
geometry_msgs::Twist cmd;
cmd.linear.x = rho;
cmd.angular.z = angular;
cmd_pub.publish(cmd);
return 0;
}
echo_effect::echo_effect(float sample_rate, float max_delay_seconds)
{
printf("initialing delay: max %f seconds\n",max_delay_seconds);
_sample_rate=sample_rate;
_effect_mix=1.0;
_delay_line_max=max_delay_seconds*sample_rate;
printf("number of samples in delay line: %d\n",_delay_line_max);
_delay_line=new float[_delay_line_max];
_read_pos=0;
_write_pos=_delay_line_max/2;
for(int i=0;i<_delay_line_max;i++)
_delay_line[i]=0.0f;
//_wg=new wave_generator(_sample_rate);
//_reached_target=true;
set_feedback(0.5);
set_read_speed(1);
set_target_length(-1);
force_length(sample_rate*max_delay_seconds*0.5);
printf(" DONE!\n");
}
void initialize()
{
// write "right" to ltl.planner
std_msgs::String msg;
std::stringstream ss;
ss << "right" << std::endl;
std::cout << "**AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAGoToWaypoint -%- Executing Main Task, elapsed_time: " << std::endl;
msg.data = ss.str();
// ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);
std::cout << "**msg:"
<< msg << std::endl;
counter++;
sleep(1.0);
//Set the stiffness so the robot can move
AL::ALValue stiffness_name("Body");
AL::ALValue stiffness(1.0f);
AL::ALValue stiffness_time(1.0f);
motion_proxy_ptr->stiffnessInterpolation(stiffness_name,
stiffness,
stiffness_time);
if( message_.type == "goto")
{
motion_proxy_ptr->moveInit();
}
set_feedback(RUNNING);
}
int Condition::executeCB(ros::Duration dt)
{
bool place_failure;
set_feedback(RUNNING);
if (!isSystemActive())
{
return 1;
}
fillParameter("/apc/task_manager/place_failure", false, place_failure);
if(place_failure)
{
set_feedback(SUCCESS);
}
else
{
set_feedback(FAILURE);
}
return 1;
}
int Condition::executeCB(ros::Duration dt)
{
Desperation desperation_mode;
set_feedback(RUNNING);
if (!isSystemActive())
{
return 1;
}
fillParameter("/apc/task_manager/desperation", desperation_mode);
if (desperation_mode == level_4)
{
set_feedback(SUCCESS);
}
else
{
set_feedback(FAILURE);
}
return 1;
}
int executeCB(ros::Duration dt)
{
std::cout << "**Walk -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**Walk -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
if(!init_)
{
set_feedback(RUNNING);
initialize();
}
double x = motion_proxy_ptr->getRobotPosition(true).at(0);
double y = motion_proxy_ptr->getRobotPosition(true).at(1);
double z = motion_proxy_ptr->getRobotPosition(true).at(2);
// Walk forward
double angular = 0.1*modulo2Pi(z_0-z);
//ROS_INFO("theta = %f",angular);
if(angular > 1) {angular = 1;}
if(angular < -1) {angular = -1;}
geometry_msgs::Twist cmd;
cmd.linear.x = 0.3; //0.5
cmd.angular.z = angular;
cmd_pub.publish(cmd);
if(sqrt((x-x_0)*(x-x_0) + (y-y_0)*(y-y_0)) > dist)
{
set_feedback(SUCCESS);
finalize();
return 1;
}
return 0;
}
int executeCB(ros::Duration dt)
{
std::cout << "**Reset Activity -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**Reset Activityt -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
if (!init_)
{
initialize();
init_ = true;
}
speech_proxy_ptr->say("Activity Reset");
msg.data = "";//I reset the activity after he's done
activity_pub.publish(msg);//so it does not say the sentence more than once
set_feedback(SUCCESS);
return 1;
}
int executeCB(ros::Duration dt)
{
std::cout << "**GoToWaypoint -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**GoToWaypoint -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
std::cout << "**Counter value:" << counter << std::endl;
if (counter > 1)
std::cout << "********************************************************************************" << std::endl;
if (!init_)
{
initialize();
init_ = true;
}
if ( (ros::Time::now() - time_at_pos_).toSec() < 0.2 )
{
if( message_.type == "goto")
{
// Goal position of ball relative to ROBOT_FRAME
float goal_x = 0.00;
float goal_y = 0.00;
float error_x = message_.x - goal_x;
float error_y = message_.y - goal_y;
if (fabs(error_x) < 0.12 && fabs(error_y) < 0.12)
{
std::cout << "Closeness count " << closeness_count << std::endl;
closeness_count++;
//If the NAO has been close for enough iterations, we consider to goal reached
if (closeness_count > 0)
{
motion_proxy_ptr->stopMove();
set_feedback(SUCCESS);
// std::cout << "sleeeping for 2 second before destroying thread" << std::endl;
// sleep(2.0);
finalize();
return 1;
}
}
else
{
closeness_count = 0;
}
//Limit the "error" in order to limit the walk speed
error_x = error_x > 0.6 ? 0.6 : error_x;
error_x = error_x < -0.6 ? -0.6 : error_x;
error_y = error_y > 0.6 ? 0.6 : error_y;
error_y = error_y < -0.6 ? -0.6 : error_y;
// float speed_x = error_x * 1.0/(2+5*closeness_count);
// float speed_y = error_y * 1.0/(2+5*closeness_count);
// float frequency = 0.1/(5*closeness_count+(1.0/(fabs(error_x)+fabs(error_y)))); //Frequency of foot steps
// motion_proxy_ptr->setWalkTargetVelocity(speed_x, speed_y, 0.0, frequency);
// ALMotionProxy::setWalkTargetVelocity(const float& x, const float& y, const float& theta, const float& frequency)
AL::ALValue walk_config;
//walk_config.arrayPush(AL::ALValue::array("MaxStepFrequency", frequency));
//Lower value of step height gives smoother walking
// std::cout << "y " << message_.y << std::endl;
if (fabs(message_.y) < 0.10)
{
// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.01));
// motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0, walk_config);
motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0);
sleep(2.0);
//motion_proxy_ptr->post.stopMove();
}
else
{
// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.005));
// motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1, walk_config);
motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1);
//sleep(3.0);
//motion_proxy_ptr->post.stopMove();
}
}
}
set_feedback(RUNNING);
return 0;
}
void initialize()
{
sleep(1.0);
set_feedback(RUNNING);
}
