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);
}
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);
// Init moving
motion_proxy_ptr->moveInit();
// Robot not detected
robotDetected = false;
}
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);
}
void NaoMarkServiceDetection::MoveHead(float angle)
{
motionProxy->angleInterpolation("HeadYaw", AL::ALValue(angle), AL::ALValue(1.0f), true);
motionProxy->stiffnessInterpolation("HeadYaw", 1.0f, 1.0f);
}