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 NaoMarkServiceDetection::callback(const std::string &key, const AL::ALValue &value, const AL::ALValue &msg)
{
AL::ALValue marks = fMemoryProxy.getData("LandmarkDetected");
if(marks.getSize() > 0 && !_isMarkFound)
{
int TimeStampField = marks[0][1];
if((int)marks[1][0][1][0] == _markToFind)
{
if((float)marks[1][0][0][3] < 0.2f)
{
if(_isAllowedToMove)
motionProxy->moveToward(0.5f,0,marks[1][0][0][1]);
_naoMarkDetected = true;
}
else
{
motionProxy->moveToward(0,0,0);
_isMarkFound = true;
}
}
else
motionProxy->moveToward(0,0,0);
}
}
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 finalize()
{
// Stop moving
motion_proxy_ptr->stopMove();
init_ = false;
deactivate();
}
void finalize()
{
// Stop rotating
motion_proxy_ptr->stopMove();
// Delete Filter
delete ic;
init_ = false;
deactivate();
}
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::backGroundService()
{
_isThreadRuning = true;
while(_isThreadRuning)
{
_naoMarkDetected = false;
Sleep((DWORD)1000);
if(!_naoMarkDetected && _markToFind != -1 && !_isMarkFound)
{
motionProxy->moveToward(0,0,0);
FindMarkArround();
}
}
}
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;
}
int executeCB(ros::Duration dt)
{
std::cout << "**GoToWaypoint -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**GoToWaypoint -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
std::cout << "**Counter value:" << counter << std::endl;
if (counter > 1)
std::cout << "********************************************************************************" << std::endl;
if (!init_)
{
initialize();
init_ = true;
}
if ( (ros::Time::now() - time_at_pos_).toSec() < 0.2 )
{
if( message_.type == "goto")
{
// Goal position of ball relative to ROBOT_FRAME
float goal_x = 0.00;
float goal_y = 0.00;
float error_x = message_.x - goal_x;
float error_y = message_.y - goal_y;
if (fabs(error_x) < 0.12 && fabs(error_y) < 0.12)
{
std::cout << "Closeness count " << closeness_count << std::endl;
closeness_count++;
//If the NAO has been close for enough iterations, we consider to goal reached
if (closeness_count > 0)
{
motion_proxy_ptr->stopMove();
set_feedback(SUCCESS);
// std::cout << "sleeeping for 2 second before destroying thread" << std::endl;
// sleep(2.0);
finalize();
return 1;
}
}
else
{
closeness_count = 0;
}
//Limit the "error" in order to limit the walk speed
error_x = error_x > 0.6 ? 0.6 : error_x;
error_x = error_x < -0.6 ? -0.6 : error_x;
error_y = error_y > 0.6 ? 0.6 : error_y;
error_y = error_y < -0.6 ? -0.6 : error_y;
// float speed_x = error_x * 1.0/(2+5*closeness_count);
// float speed_y = error_y * 1.0/(2+5*closeness_count);
// float frequency = 0.1/(5*closeness_count+(1.0/(fabs(error_x)+fabs(error_y)))); //Frequency of foot steps
// motion_proxy_ptr->setWalkTargetVelocity(speed_x, speed_y, 0.0, frequency);
// ALMotionProxy::setWalkTargetVelocity(const float& x, const float& y, const float& theta, const float& frequency)
AL::ALValue walk_config;
//walk_config.arrayPush(AL::ALValue::array("MaxStepFrequency", frequency));
//Lower value of step height gives smoother walking
// std::cout << "y " << message_.y << std::endl;
if (fabs(message_.y) < 0.10)
{
// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.01));
// motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0, walk_config);
motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0);
sleep(2.0);
//motion_proxy_ptr->post.stopMove();
}
else
{
// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.005));
// motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1, walk_config);
motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1);
//sleep(3.0);
//motion_proxy_ptr->post.stopMove();
}
}
}
set_feedback(RUNNING);
return 0;
}
void NaoMarkServiceDetection::MoveHead(float angle)
{
motionProxy->angleInterpolation("HeadYaw", AL::ALValue(angle), AL::ALValue(1.0f), true);
motionProxy->stiffnessInterpolation("HeadYaw", 1.0f, 1.0f);
}
void NaoMarkServiceDetection::TurnToDetectedMark(float angle)
{
motionProxy->moveTo(0,0,angle);
MoveHead(0);
_isAllowedToMove = true;
}
std::pair<float, float> worldBallPosFromImgCoords(AL::ALMotionProxy motionProxy,
std::pair<int, int> ballPosCam,
int imgWidth, int imgHeight,
int camera)
{
std::string cameraName = "CameraTop";
if (camera == 1)
{
cameraName = "CameraBottom";
}
// Image coordinates of ball
int u = ballPosCam.first;
int v = ballPosCam.second;
// Angles of observation of the ball
float phi = ((float)u-((float)imgWidth)/2)/(float)imgWidth * img_WFOV;
float theta = ((float)v-((float)imgHeight)/2)/(float)imgHeight * img_HFOV;
// Select the right coordinates for the NAO system!
// x outward from camera, y to the left and z vertically upwards
// Coefficients for line-equation going from NAO camera to the ball
float b_c = -sin(phi);
float c_c = -sin(theta);
float a_c = sqrt((cos(phi)*cos(phi)+cos(theta)*cos(theta))/2);
int space = 2; //FRAME_ROBOT
bool useSensorValues = true;
std::vector<float> transVec =
motionProxy.getTransform(cameraName, space, useSensorValues); // Transform camera -> FRAME_ROBOT
std::vector<float> cameraPos =
motionProxy.getPosition(cameraName, space, useSensorValues); // Camera position in FRAME_ROBOT
// std::cout << "Position of bottom camera: " << std::endl;
// std::cout << cameraPos.at(0) << " " << cameraPos.at(1) << " " << cameraPos.at(2) << std::endl;
// std::cout << cameraPos.at(3) << " " << cameraPos.at(4) << " " << cameraPos.at(5) << std::endl;
// Put the camera transform into an Eigen matrix for easy multiplication
Eigen::Matrix4f trans;
trans <<
transVec[0] , transVec[1] , transVec[2] , transVec[3] ,
transVec[4] , transVec[5] , transVec[6] , transVec[7] ,
transVec[8] , transVec[9] , transVec[10], transVec[11],
transVec[12], transVec[13], transVec[14], transVec[15];
Eigen::Vector4f vec(a_c, b_c, c_c, 1);
// Transform the line equation from NAO camera coordinate system into FRAME_ROBOT
Eigen::Vector4f transformedLine = trans*vec;
// std::cout << "trans*vec = " << transformedLine << std::endl;
// Solve line-plane intersection with plane at z (floor)
// Solution from Wikipedia line-plane intersection article
float z = 0.00;
Eigen::Matrix3f lineMat;
lineMat <<
cameraPos.at(0)-transformedLine[0], 1.0-0.0, 0.0-0.0,
cameraPos.at(1)-transformedLine[1], 0.0-0.0, 1.0-0.0,
cameraPos.at(2)-transformedLine[2], z - z, z - z;
Eigen::Vector3f lineVec;
lineVec << cameraPos.at(0)-0.0, cameraPos.at(1)-0.0, cameraPos.at(2)-z;
Eigen::Vector3f txy = lineMat.inverse()*lineVec;
std::cout << "Ball is at (x, y): (" << txy[1] << ", " << txy[2] << ")" << std::endl;
std::pair<float, float> return_value(txy[1], txy[2]);
return return_value; //Return ball position (x, y)
}
