void LocalMoveBase::movebase(void)
{
std_msgs::String state;
bool done=false;
ros::Rate r(RATE);
while (ros::ok())
{
if(NewPath)
{
ChoosePath();//选择路线
PublishState("StartMoveZ");
NewPath=false;
int i=1;
int length=MPath[FIANLPATH].poses.size();
ROS_INFO("has %d goals",length-1);
ccr.runBehavior();
while (i<length)
{
//PublishState("RotateZ");
ROS_INFO("--------------------->");
if(i<(length-2))
{
//判断下一个目标点是否有效
done=mLocalCostmap->isNextGoalValid(MPath[FIANLPATH],i-1);
if(!done)
{
break;
}
}
done=moveToGoal_Forward(MPath[FIANLPATH].poses[i-1],MPath[FIANLPATH].poses[i],i);
if(!done)
{
break;
}
//清空一下周围地图
ccr.runBehavior();
i+=1;
ros::spinOnce();
}
if(done)
{
if(rotateToGoalDirection(MPath[FIANLPATH].poses[i-2],MPath[FIANLPATH].poses[i-1]))
{
PublishState("GoalReachedZ");
ccr.runBehavior();
}
}
else
{
}
}
ros::spinOnce();
r.sleep();
}
}
/*
* Give error only in yaw, assure rov force command only in yaw.
*/
TEST_F(QuaternionPdControllerTest, OnlyYaw)
{
uranus_dp::SetControlMode srv;
srv.request.mode = ControlModes::POSITION_HOLD;
if (!modeClient.call(srv))
ROS_ERROR("Failed to call service set_control_mode. New mode POSITION_HOLD not set.");
uranus_dp::SetControllerGains gainSrv;
gainSrv.request.a = 1;
gainSrv.request.b = 30;
gainSrv.request.c = 200;
if (!gainClient.call(gainSrv))
ROS_ERROR("Failed to call service set_controller_gains.");
Eigen::Vector3d p(0,0,0);
Eigen::Quaterniond q(1,0,0,0);
Eigen::Vector3d v(0,0,0);
Eigen::Vector3d omega(0,0,0);
Eigen::Quaterniond q_sp(0.923879532511287,0,0,0.382683432365090); // Equal to 45 degrees yaw
q_sp.normalize();
PublishState(p, q, v, omega);
PublishSetpoint(p, q_sp);
ros::Duration(0.5).sleep(); // Controller runs at 10 Hz, must give it time to compute new value.
WaitForMessage();
EXPECT_NEAR(tau(0), 0, EPSILON);
EXPECT_NEAR(tau(1), 0, EPSILON);
EXPECT_NEAR(tau(2), 0, EPSILON);
EXPECT_NEAR(tau(3), 0, EPSILON);
EXPECT_NEAR(tau(4), 0, EPSILON);
EXPECT_NEAR(tau(5), 76.536686473017951, EPSILON);
}
/*
* Give error only in sway, assure rov force command only in sway.
*/
TEST_F(QuaternionPdControllerTest, OnlySway)
{
uranus_dp::SetControlMode srv;
srv.request.mode = ControlModes::POSITION_HOLD;
if (!modeClient.call(srv))
ROS_ERROR("Failed to call service set_control_mode. New mode POSITION_HOLD not set.");
uranus_dp::SetControllerGains gainSrv;
gainSrv.request.a = 1;
gainSrv.request.b = 30;
gainSrv.request.c = 200;
if (!gainClient.call(gainSrv))
ROS_ERROR("Failed to call service set_controller_gains.");
Eigen::Vector3d p(0,0,0);
Eigen::Quaterniond q(1,0,0,0);
Eigen::Vector3d v(0,0,0);
Eigen::Vector3d omega(0,0,0);
Eigen::Vector3d p_sp(0,1,0);
PublishState(p, q, v, omega);
PublishSetpoint(p_sp, q);
ros::Duration(0.5).sleep(); // Controller runs at 10 Hz, must give it time to compute new value.
WaitForMessage();
EXPECT_NEAR(tau(0), 0, EPSILON);
EXPECT_NEAR(tau(1), 30, EPSILON);
EXPECT_NEAR(tau(2), 0, EPSILON);
EXPECT_NEAR(tau(3), 0, EPSILON);
EXPECT_NEAR(tau(4), 0, EPSILON);
EXPECT_NEAR(tau(5), 0, EPSILON);
}
/*
* Input zero speed and nonzero pose. Input same pose state and pose setpoint. Expect zero output.
*/
TEST_F(QuaternionPdControllerTest, ZeroErrorZeroOutput)
{
uranus_dp::SetControlMode srv;
srv.request.mode = ControlModes::POSITION_HOLD;
if (!modeClient.call(srv))
ROS_ERROR("Failed to call service set_control_mode. New mode POSITION_HOLD not set.");
Eigen::Vector3d p(1,2,3);
Eigen::Quaterniond q(4,5,6,7);
q.normalize();
Eigen::Vector3d v(0,0,0);
Eigen::Vector3d omega(0,0,0);
PublishState(p, q, v, omega);
PublishSetpoint(p, q);
WaitForMessage();
EXPECT_NEAR(tau(0), 0, EPSILON);
EXPECT_NEAR(tau(1), 0, EPSILON);
EXPECT_NEAR(tau(2), 0, EPSILON);
EXPECT_NEAR(tau(3), 0, EPSILON);
EXPECT_NEAR(tau(4), 0, EPSILON);
EXPECT_NEAR(tau(5), 0, EPSILON);
}
//#转动到目标点的方向
bool LocalMoveBase::rotateToGoalDirection(const geometry_msgs::PoseStamped &lastpose,
const geometry_msgs::PoseStamped ¤tpose)
{
double cAngle=mMath.getYaw();//获取当前机器人方向
double goalAngle=mMath.quat_to_angle(currentpose);//获取最终方向
if(!(mLocalCostmap->canRobotRotate(currentpose.pose.position.x,currentpose.pose.position.y,cAngle,goalAngle)))
{
ROS_INFO("ERROR! COLLISION WILL Happen");
PublishState("RotateErrorZ");
return false;
}
rotateToAngle(goalAngle);
return true;
}
//#传入目标位置 将机器人转动到向该目标点运动的方向
bool LocalMoveBase::rotateToMoveDirection(const geometry_msgs::PoseStamped ¤tpose,
const geometry_msgs::PoseStamped &goalpose)
{
//#计算目标点与当前点的全局坐标系下的角度值
double cx,cy,cAngle;
mMath.getGolbalPosition(cx,cy,cAngle);
ROS_INFO("now position x %f y %f ",cx,cy);
double goalAngle=mMath.canculateAngle(goalpose.pose.position.x,goalpose.pose.position.y,cx,cy);
if(!(mLocalCostmap->canRobotRotate(currentpose.pose.position.x,currentpose.pose.position.y,cAngle,goalAngle)))
{
ROS_INFO("ERROR! COLLISION WILL Happen");
PublishState("RotateErrorZ");
return false;
}
rotateToAngle(goalAngle);
return true;
}
/*
* \brief Updates the diagnostic component. Diagnostics
* Publishes the state
*
*/
void AgvsPad::Update(){
PublishState();
}
bool LocalMoveBase::moveToGoal_Forward(const geometry_msgs::PoseStamped ¤tpose,const geometry_msgs::PoseStamped &goalpose,int goalpose)
{
//旋转至运动方向
bool done=rotateToMoveDirection(currentpose,goalpose);
if(!done)
{
ROS_INFO("LAST TIME Prediction ERROR");
getGlobalPath();
return false;
}
if(NewPath) return false;//如果出现了新的路径立即返回
//#获取机器人目标的位置 角度
if(!mLocalCostmap->isLineValid(currentpose,goalpose))
{
ROS_INFO("TISH next PATH is INvalid");
return false;
}
double GX=goalpose.pose.position.x;
double GY=goalpose.pose.position.y;
//获取机器人的起始坐标
double x_start,y_start;
mMath.getGolbalPosition(x_start,y_start);
//到目标点的位置
double goal_distance=mMath.canculateDistance(GX,GY,x_start,y_start);
ROS_INFO("goal_distance:%f",goal_distance);
geometry_msgs::Twist move_cmd;
double distance=0.0;
double x_current,y_current;
isRobotMoving=true;
ros::Rate r(RATE);
//在运动的过程中实时检测下一个目标点的有效性
int i=0;
while (distance<goal_distance and ros::ok())
{
//检测3m以内目标点的有效性
i++;
if(i%int(RATE)==0)
{
if(!mLocalCostmap-> isMetersPathValid(6.0,MPath[FIANLPATH],nextpose))
{
isForwardObstacle=true;
PublishState("MoveErrorZ");
//申请一条新的路径
getGlobalPath();
}
}
ros::spinOnce();
cmd_vel_pub.publish(move_cmd);
r.sleep();
//获取当前的位置
mMath.getGolbalPosition(x_current,y_current);
distance=mMath.canculateDistance(x_current,y_current,x_start,y_start);
bool up=true;
if(goal_distance<MODE1_DIS)
{
if (!((distance<(goal_distance/2.0)) and not NewPath and not isForwardObstacle)) {up=false;}
}
else
{
if (!(((goal_distance-distance)>MODE2_DIS)and not NewPath and not isForwardObstacle)) {up=false;}
}
if(up)
{
if (move_cmd.linear.x<MAX_LINEAR_X) {move_cmd.linear.x+=ACC_LINEAR_X/RATE;}
}
else
{
//这个是减速环节 快到达位置的时候让机器人停下来 所以此处应该判断下一处是否应该转向 如果不转向
//则机器人不需停止
//
if (move_cmd.linear.x>MIN_LINEAR_X)
{
move_cmd.linear.x-=ACC_LINEAR_X/RATE;
}
else
{
if(NewPath)
{
//最后让机器人停下来
PublishMoveStopCMD();
return false;
}
if(isForwardObstacle)
{
PublishMoveStopCMD();
return false;
}
}
}
}
geometry_msgs::Twist move_stop_cmd;
cmd_vel_pub.publish(move_stop_cmd);
return true;
}
