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); }