void initialize()
{
init_ = true;
// Enable stiffness
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);
// Stand
//robotPosture->goToPosture("Stand",0.5f);
// Init moving
motion_proxy_ptr->moveInit();
// Initial Odometry
x_0 = motion_proxy_ptr->getRobotPosition(true).at(0);
y_0 = motion_proxy_ptr->getRobotPosition(true).at(1);
z_0 = motion_proxy_ptr->getRobotPosition(true).at(2);
}
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;
}