tf::Stamped<tf::Pose> getGripperPose(const door_msgs::Door& door, double angle, double dist)
{
Vector x_axis(1,0,0);
// get hinge point
Vector hinge, frame_vec;
if (door.hinge == door_msgs::Door::HINGE_P1){
hinge = Vector(door.door_p1.x, door.door_p1.y, door.door_p1.z);
frame_vec = Vector(door.frame_p2.x - door.frame_p1.x, door.frame_p2.y - door.frame_p1.y, door.frame_p2.z - door.frame_p1.z);
}
else if (door.hinge == door_msgs::Door::HINGE_P2){
hinge = Vector(door.door_p2.x, door.door_p2.y, door.door_p2.z);
frame_vec = Vector(door.frame_p1.x - door.frame_p2.x, door.frame_p1.y - door.frame_p2.y, door.frame_p1.z - door.frame_p2.z);
}
// get gripper pos
frame_vec.Normalize();
frame_vec = frame_vec * dist;
Rotation rot_angle = Rotation::RotZ(angle);
Vector gripper_pos = hinge + (rot_angle * frame_vec);
tf::Stamped<tf::Pose> gripper_pose;
Vector normal_frame = getFrameNormal(door);
gripper_pose.frame_id_ = door.header.frame_id;
gripper_pose.stamp_ = door.header.stamp;
gripper_pose.setOrigin( tf::Vector3(gripper_pos(0), gripper_pos(1), gripper_height));
gripper_pose.setRotation( tf::createQuaternionFromRPY(getVectorAngle(x_axis, normal_frame)+angle, 0, 0) );
return gripper_pose;
}
double getDoorAngle(const door_msgs::Door& door)
{
Vector frame_vec(door.frame_p1.x-door.frame_p2.x, door.frame_p1.y-door.frame_p2.y, door.frame_p1.z-door.frame_p2.z);
Vector door_vec(door.door_p1.x-door.door_p2.x, door.door_p1.y-door.door_p2.y, door.door_p1.z-door.door_p2.z);
double angle = getVectorAngle(frame_vec, door_vec);
// validity check
if (door.rot_dir == door_msgs::Door::ROT_DIR_CLOCKWISE && angle > eps_angle)
ROS_DEBUG("Door angle is positive, but door message specifies it turns clockwise");
if (door.rot_dir == door_msgs::Door::ROT_DIR_COUNTERCLOCKWISE && angle < -eps_angle)
ROS_DEBUG("Door angle is negative, but door message specifies it turns counter-clockwise");
return angle;
}
/*
* Get the maximum angle between the center and vertices in 3D.
*/
double
Triangle::nPointsMaxAngle(vector<Point3d> vertices){
double max_theta = -1.0;
double theta = -1.0;
cv::Point3d center = nPointsCenter3d(vertices);
for(int i = 0;i < vertices.size();i++){
theta = getVectorAngle(vertices[i], center);
if(theta > max_theta || max_theta == -1.0){
max_theta = theta;
}
}
return max_theta;
}
/*
* Get the maximum angle between the center and vertices in 3D.
*/
double
Triangle::getMaxAngle(vector<Point3d> vertices){
size_t size = 3;
double max_theta = -1.0;
double theta = -1.0;
cv::Point3d center = triangleCenter3d(vertices);
for(int i = 0;i < 3;i++){
theta = getVectorAngle(vertices[i], center);
if(theta > max_theta || max_theta == -1.0){
max_theta = theta;
}
}
return max_theta;
}
tf::Stamped<tf::Pose> getHandlePose(const door_msgs::Door& door, int side)
{
Vector x_axis(1,0,0);
double dist = sqrt(pow(door.frame_p1.x - door.handle.x,2)+pow(door.frame_p1.y - door.handle.y,2));
if(door.hinge == door_msgs::Door::HINGE_P2)
{
dist = sqrt(pow(door.frame_p2.x - door.handle.x,2)+pow(door.frame_p2.y - door.handle.y,2));
}
double angle = getDoorAngle(door);
// get hinge point
Vector hinge, frame_vec;
if (door.hinge == door_msgs::Door::HINGE_P1){
hinge = Vector(door.door_p1.x, door.door_p1.y, door.door_p1.z);
frame_vec = Vector(door.frame_p2.x - door.frame_p1.x, door.frame_p2.y - door.frame_p1.y, door.frame_p2.z - door.frame_p1.z);
}
else if (door.hinge == door_msgs::Door::HINGE_P2){
hinge = Vector(door.door_p2.x, door.door_p2.y, door.door_p2.z);
frame_vec = Vector(door.frame_p1.x - door.frame_p2.x, door.frame_p1.y - door.frame_p2.y, door.frame_p1.z - door.frame_p2.z);
}
// get gripper pos
frame_vec.Normalize();
frame_vec = frame_vec * dist;
Rotation rot_angle = Rotation::RotZ(angle);
Vector handle_pos = hinge + (rot_angle * frame_vec);
tf::Stamped<tf::Pose> handle_pose;
Vector normal_frame = getFrameNormal(door);
if(side == -1)
{
normal_frame = -normal_frame;
}
handle_pose.frame_id_ = door.header.frame_id;
handle_pose.stamp_ = door.header.stamp;
handle_pose.setOrigin( tf::Vector3(handle_pos(0), handle_pos(1), gripper_height));
handle_pose.setRotation( tf::createQuaternionFromRPY(getVectorAngle(x_axis, normal_frame)+angle, 0, 0) );
tf::Pose gripper_rotate(tf::createQuaternionFromRPY(0.0,0.0,M_PI/2.0),tf::Vector3(0.0,0.0,0.0));
handle_pose.mult(handle_pose,gripper_rotate);
return handle_pose;
}
tf::Stamped<tf::Pose> getRobotPose(const door_msgs::Door& door, double dist)
{
Vector x_axis(1,0,0);
// get vector to center of frame
Vector frame_1(door.frame_p1.x, door.frame_p1.y, door.frame_p1.z);
Vector frame_2(door.frame_p2.x, door.frame_p2.y, door.frame_p2.z);
Vector frame_center = (frame_2 + frame_1)/2.0;
// get robot pos
Vector frame_normal = getFrameNormal(door);
Vector robot_pos = frame_center + (frame_normal * dist);
tf::Stamped<tf::Pose> robot_pose;
robot_pose.frame_id_ = door.header.frame_id;
robot_pose.stamp_ = door.header.stamp;
robot_pose.setOrigin( tf::Vector3(robot_pos(0), robot_pos(1), robot_pos(2)));
robot_pose.setRotation( tf::createQuaternionFromRPY(getVectorAngle(x_axis, frame_normal), 0, 0) );
return robot_pose;
}
int Character::update(int skip){
//detect keyevent
int success = 0;
int newState = 0;
if(FyCheckHotKeyStatus(FY_UP)){
newState = newState|MOVE_FORWARD;
std::cout<<"up key\n";
}
if(FyCheckHotKeyStatus(FY_DOWN)){
newState = newState|MOVE_BACKWARD;
std::cout<<"down key\n";
}
if(FyCheckHotKeyStatus(FY_LEFT)){
newState = newState|MOVE_LEFT;
std::cout<<"left key\n";
}
if(FyCheckHotKeyStatus(FY_RIGHT)){
newState = newState|MOVE_RIGHT;
std::cout<<"right key\n";
}
std::cout.flush();
switch(newState){
case (int)MotionState::IDLE:
case (int)MotionState::MOVE_FORWARD:
case (int)MotionState::MOVE_BACKWARD:
case (int)MotionState::MOVE_LEFT_FORWARD:
case (int)MotionState::MOVE_LEFT_BACKWARD:
case (int)MotionState::MOVE_RIGHT_FORWARD:
case (int)MotionState::MOVE_RIGHT_BACKWARD:
case (int)MotionState::MOVE_LEFT:
case (int)MotionState::MOVE_RIGHT:
m_curState = (MotionState)newState;
break;
default:
m_curState = m_preState;
}
//animation
if(m_mapState2Action[m_curState] != m_curActionId){
m_curActionId = m_mapState2Action[m_curState];
m_actor.SetCurrentAction(NULL, 0, m_curActionId, 5.0f);
m_actor.Play(START, 0.0f, FALSE, TRUE);
}else{
m_actor.Play(LOOP, (float)skip, FALSE, TRUE);
}
//check motion state
FnObject dummy(m_dummyCameraId);
float fDummyFDir[3];
float fDummyUDir[3];
float fDummyPos[3];
float fDstDir[3];
dummy.GetDirection(fDummyFDir, fDummyUDir);
switch(m_curState){
case MotionState::IDLE:
break;
case MotionState::MOVE_FORWARD:
case MotionState::MOVE_BACKWARD:
case MotionState::MOVE_LEFT_FORWARD:
case MotionState::MOVE_LEFT_BACKWARD:
case MotionState::MOVE_RIGHT_FORWARD:
case MotionState::MOVE_RIGHT_BACKWARD:
getDstDirection(fDummyFDir, fDummyUDir, fDstDir, m_curState);
float fAngleDiff;
getVectorAngle(m_fDir3, fDstDir, fAngleDiff);
if(std::fabs(fAngleDiff) <= m_rotateVel){
m_actor.TurnRight(fAngleDiff);
//int success = m_actor.MoveForward(m_moveVel, TRUE, FALSE, 2, TRUE );
success = m_actor.MoveForward(m_moveVel, TRUE, false, FALSE, TRUE);
}else{
float fSign = fAngleDiff/std::fabs(fAngleDiff);
success = m_actor.TurnRight(fSign*m_rotateVel);
}
break;
case MotionState::MOVE_LEFT:
dummy.GetPosition(fDummyPos);
getDstDirection(fDummyFDir, fDummyUDir, fDstDir, m_curState);
getVectorAngle(m_fDir3, fDstDir, fAngleDiff);
if(std::fabs(fAngleDiff) <= m_rotateVel){
m_actor.TurnRight(fAngleDiff);
float fDistance;
getPositionDist2D(fDummyPos, m_fPos3, fDistance);
double fHalfAngle = -asin(0.5*m_moveVel/fDistance)*180/3.14159265;
m_actor.TurnRight(fHalfAngle);
success = m_actor.MoveForward(m_moveVel, TRUE, false, FALSE, TRUE);
// dummy.TurnRight(2*fHalfAngle);
}else{
float fSign = fAngleDiff/std::fabs(fAngleDiff);
success = m_actor.TurnRight(fSign*m_rotateVel);
}
break;
case MotionState::MOVE_RIGHT:
dummy.GetPosition(fDummyPos);
getDstDirection(fDummyFDir, fDummyUDir, fDstDir, m_curState);
getVectorAngle(m_fDir3, fDstDir, fAngleDiff);
if(std::fabs(fAngleDiff) <= m_rotateVel){
m_actor.TurnRight(fAngleDiff);
float fDistance;
getPositionDist2D(fDummyPos, m_fPos3, fDistance);
double fHalfAngle = asin(0.5*m_moveVel/fDistance)*180/3.14159265;
m_actor.TurnRight(fHalfAngle);
success = m_actor.MoveForward(m_moveVel, TRUE, false, FALSE, TRUE);
// dummy.TurnRight(2*fHalfAngle);
}else{
float fSign = fAngleDiff/std::fabs(fAngleDiff);
success = m_actor.TurnRight(fSign*m_rotateVel);
}
break;
}
//update state
m_preState = m_curState;
m_actor.GetPosition(m_fPos3);
m_actor.GetDirection(m_fDir3, m_uDir3);
collision = success;
return success;
}