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; }
int main(int argc, char** argv) { ros::init(argc, argv,"nao_behavior"); ros::NodeHandle nh; // Robot parameters ros::NodeHandle pnh("~"); std::string NAO_IP; int NAO_PORT; pnh.param("NAO_IP",NAO_IP,std::string("127.0.0.1")); pnh.param("NAO_PORT",NAO_PORT,int(9559)); // Enable stiffness AL::ALMotionProxy* motion_proxy_ptr; motion_proxy_ptr = new AL::ALMotionProxy(NAO_IP,NAO_PORT); 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); // Behavior manager proxy AL::ALBehaviorManagerProxy* behavior_proxy_ptr; behavior_proxy_ptr = new AL::ALBehaviorManagerProxy(NAO_IP,NAO_PORT); // Install behavior behavior_proxy_ptr->installBehavior("/home/olivier/ros_workspace/nao_custom/behavior_hello/behavior.xar"); // Behavior list std::vector<std::string> list = behavior_proxy_ptr->getInstalledBehaviors(); printf("Behaviors list:\n"); for(size_t i = 0; i < list.size(); i++) { std::cout << list.at(i) << std::endl; } // Run behavior behavior_proxy_ptr->runBehavior("behavior_hello"); ros::Rate loop_rate(100); while(ros::ok()) { loop_rate.sleep(); ros::spinOnce(); } return 0; }
BTAction(std::string name) : as_(nh_, name, boost::bind(&BTAction::executeCB, this, _1), false), action_name_(name) { as_.start(); robot_ip = "192.168.0.188"; std::cout << "Robot ip to use is: " << robot_ip << std::endl; motion_proxy_ptr = new AL::ALMotionProxy("192.168.0.188", 9559); //put this in init() 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); //motion_proxy_ptr->moveInit(); }
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 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(); // Init bearing std::vector<float> odometry = motion_proxy_ptr->getRobotPosition(useSensorValues); theta_temp = odometry.at(2); r.theta = angle(p.x-r.x,p.y-r.y); }