TEST(TestSuite, testCaseRpy) {
//std::cout << "Check row versus column major matrice\n";
// TestRpy(0.1, 0, 0);
double r = 90, p = 0, y = 90;
printf(" IN: %f %f %f\n", r, p, y);
tf::Matrix3x3 m = GetTfRotationMatrix(Deg2Rad(r), Deg2Rad(p), Deg2Rad(y));
GetRPY(m, r, p, y);
printf("OUT: %f %f %f\n", Rad2Deg(r), Rad2Deg(p), Rad2Deg(y));
tf::Vector3 xaxis, zaxis;
GetHighland(m, xaxis, zaxis);
printf("%f %f %f\n", xaxis.getX(), xaxis.getY(), xaxis.getZ());
printf("%f %f %f\n", zaxis.getX(), zaxis.getY(), zaxis.getZ());
}
//動作部
void EyeContact::moveThread()
{
ros::NodeHandle nmv;
ros::Rate rate(30);
bool state1_back = false;
geometry_msgs::Point origin_point;
geometry_msgs::Quaternion origin_quat;
double roll, pitch, yaw;
now_pose =
ros::topic::waitForMessage<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose");
origin_point = now_pose->pose.pose.position;
origin_quat = now_pose->pose.pose.orientation;
GetRPY(origin_quat, roll, pitch, yaw);
double rot = 0;
//tell the action client that we want to spin a thread by default
MoveBaseClient ac("move_base", true);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
move_state = false;
}
bool send_goal = true;
while(nmv.ok())
{
if( move_state )
{
geometry_msgs::Twist cmd_vel;
move_base_msgs::MoveBaseGoal goal;
if(dir_horizon > 0)
{
rot = -180.+dir_horizon;
}
else if(dir_horizon < 0)
{
rot = 180.+dir_horizon;
}
else
rot = 0;
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
if(state==STATE1)
{
goal.target_pose.pose.position = origin_point;
goal.target_pose.pose.orientation = origin_quat;
}
else if(state==STATE4)
{
goal.target_pose.pose.position = origin_point;
double now_th = yaw/2.*(M_PI/180.);
double del_th = rot/2.*(M_PI/180.);
goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw((yaw+rot)*M_PI/180.);
}
if(send_goal)
{
ac.sendGoal(goal);
send_goal = false;
}
else
{
if(state==STATE2 || state==STATE3)
{
ROS_INFO("rotation stop because face get");
ac.cancelAllGoals();
move_state = false;
send_goal = true;
now_pose =
ros::topic::waitForMessage<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose");
origin_point = now_pose->pose.pose.position;
origin_quat = now_pose->pose.pose.orientation;
GetRPY(origin_quat, roll, pitch, yaw);
}
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("OK");
move_state = false;
send_goal = true;
}
else if(ac.getState() == actionlib::SimpleClientGoalState::ABORTED)
{
ROS_ERROR("MISS");
move_state = false;
send_goal = true;
}
}
}
rate.sleep();
}
}
//! GetEulerZYX gets the euler ZYX parameters of a rotation :
//! First rotate around Z with alfa,
//! then around the new Y with beta, then around
//! new X with gamma.
//!
//! Range of the results of GetEulerZYX :
//! -PI <= alfa <= PI
//! -PI <= gamma <= PI
//! -PI/2 <= beta <= PI/2
//!
//! Closely related to RPY-convention.
inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
GetRPY(Gamma,Beta,Alfa);
}