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