void Pheeno::PIDWayPointControl(float u1, float u2, float desVelocity, float timeStep){
/*Causes Pheeno to move to waypoint located at (u1,u2) assuming inial
condition X0,Y0,A0 defined initially and updated by the robot's
odometry.*/
if (millis() - PIDWayPointTimeStart >= timeStep){
float PIDTimeStep = (millis()-PIDWayPointTimeStart)/1000;
float desHeading = atan2((u2-botYPos), (u1-botXPos));// rad
//Calculate Errors for PID control
errorAng = wrapToPi(desHeading - botA);
integralAng = wrapToPi(integralAng + errorAng * PIDTimeStep);
diffAng = wrapToPi((oldErrorAng - errorAng) / PIDTimeStep);
oldErrorAng = errorAng;
PIDWayPointW = rotationalDigitalK*(kpAng*errorAng + kiAng*integralAng + kdAng*diffAng);
PIDWayPointTimeStart = millis();
}
// motorLVel and motorRVel are in rad/s
float motorLVel = convertUnicycleToLeftMotor(desVelocity, PIDWayPointW);
float motorRVel = convertUnicycleToRightMotor(desVelocity, PIDWayPointW);
PIDMotorControl(motorLVel, motorRVel);
}
float Pheeno::wrapTo2Pi(float rad){
//Converts radian angle [0,2*pi)
float output = wrapToPi(rad);
if (output < 0){
output = output + 2*pi;
}
return output;
}
void Pheeno::sensorFusionPositionUpdate(float timeStep, float northOffset){
if (millis() - positionUpdateTimeStart >= timeStep){
timeStep = (millis() - positionUpdateTimeStart)/1000; //Convert ms to s
positionUpdateTimeStart = millis();
readEncoders();
int countL = encoderCountL;
int countR = encoderCountR;
float Dr = pi * wheelDiameter * (countR - oldSFPUEncoderCountR) / (encoderCountsPerRotation * motorGearRatio); // Linear distance right wheel has rotated.
float Dl = pi * wheelDiameter * (countL - oldSFPUEncoderCountL) / (encoderCountsPerRotation * motorGearRatio); // Linear distance left wheel has rotated.
//Check integer roll over!
if (countL < 0 && oldSFPUEncoderCountL > 0){
Dl = pi*wheelDiameter * ((countL - (-32768)) + (32767 - oldSFPUEncoderCountL)) / (encoderCountsPerRotation * motorGearRatio); // Linear distance left wheel has rotated.
}
if (countR < 0 && oldEPUEncoderCountR > 0){
Dr = pi*wheelDiameter * ((countR - (-32768)) + (32767 - oldSFPUEncoderCountR)) / (encoderCountsPerRotation * motorGearRatio); // Linear distance left wheel has rotated.
}
float Dc = (Dr + Dl)/2; //Distance center of bot has moved read by encoders.
oldSFPUEncoderCountR = countR;
oldSFPUEncoderCountL = countL;
float botD = 0.8 * Dc + 0.2 * botVel * timeStep;
readCompass(northOffset);
if (botA0 > -pi/2 && botA0 < pi/2){
botA = 0.9 * wrapToPi(botA + (Dr - Dl)/axelLength) + 0.1 * wrapToPi(deg2rad(IMUOrientation));
}
else{
botA = 0.9 * wrapTo2Pi(botA + (Dr - Dl)/axelLength) + 0.1 * wrapTo2Pi(deg2rad(IMUOrientation));
}
botXPos += botD * cos(botA0 + (botA-botA0)/2);
botYPos += botD * sin(botA0 + (botA-botA0)/2);
readAccel();
botVel = 0.95 * Dc/timeStep + 0.05 * (botVel + IMUACCY * timeStep);
botA0 = botA;
}
}
TPose2D CGyro::ReadPose(mrpt::system::TTimeStamp *timestamp)
{
EnterCriticalSection(&m_cs);
if(timestamp)
{
*timestamp = m_timestamp;
}
TPose2D poseBase = m_tpose2D;
LeaveCriticalSection(&m_cs);
// 简单的相加不行！必须写下去！角度决定加减！
// TPose2D poseReal(poseBase.x + m_tposeCalib.x,
// poseBase.y + m_tposeCalib.y,
// poseBase.phi + m_tposeCalib.phi);
// poseReal.phi = wrapToPi(poseReal.phi);
poseBase.phi = wrapToPi(poseBase.phi);
return poseBase;
}
int hl_prospective( float *vec, float px, float py,
float vx_own, float vy_own,
float vx_obst, float vy_obst,
float dt, float max, int length, float radius)
{
int i, j;
float v, ang, vox, voy, temp;
cart2polar(vx_own, vy_own, &v, &temp);
int flag = 0;
for (i = 0; i < length; i++)
{
j = 1;
ang = -M_PI + (2*M_PI/length)*i;
wrapToPi(&ang);
polar2cart(v, ang, &vox, &voy);
while(j*dt < max)
{
vec[i] = j*dt;
flag = hl_collisiontest( px, py, vox, voy, vx_obst, vy_obst, j*dt, radius);
if (flag)
break;
else
j++;
}
}
return flag;
}
int CPathAgent::Start()
{
if(m_bAgentStarted)
return -1;
////////////////////////// 配置文件 RobotCfg.ini /////////////////////
{
string iniFile = string(INI_FILE);
mrpt::utils::CConfigFile cfgFile(iniFile);
m_dCalibCenterDistance = cfgFile.read_double("PathAgent", "CalibCenterDistance", 0.85);
}
////////////////////////// 路径文件 Path.ini /////////////////////////
m_Path_ini_file = string(ROOT_DIR) + string("INI/Path.ini");
if(mrpt::system::fileExists(m_Path_ini_file) == false) // 路径文件找不到
{
cout << "[!!!] PathAgent: Start():找不到路径点文件！：" << m_Path_ini_file << endl;
return -1;
}
// 读入路径点总数量
{
mrpt::utils::CConfigFile PP_File(m_Path_ini_file);
m_iPathPointSum = PP_File.read_int("PathPoint", "PathPointSum", 0);
if (m_iPathPointSum <= 10)
{
cout<<"[!!!] PathAgent: Start(): 路径点数量不能为0！："
<<m_Path_ini_file<<endl;
return -1;
}
}
////////////////////////// 路标文件 LandMark.ini /////////////////////////
m_Landmark_ini_file = string(ROOT_DIR) + string("INI/LandMark.ini");
if(mrpt::system::fileExists(m_Landmark_ini_file) == false)
{
cout << "[!!!] PathAgent: Start():找不到路标配置文件！：" << m_Landmark_ini_file << endl;
return -1;
}
// 读入视觉路标的大地坐标
{
mrpt::utils::CConfigFile LM_cfgFile(m_Landmark_ini_file);
m_iLandMarkNum = LM_cfgFile.read_int("LandMark", "LandMarkNum", 0, false);
if ((m_iLandMarkNum <= 0) || (m_iLandMarkNum > 100))
{
cout<<"[!!!] PathAgent: Start(): 路标数量必须在1~100之间！："
<<m_Landmark_ini_file<<endl;
return -1;
}
m_pLandMarkPose = new TPose2D[m_iLandMarkNum];
// 开始读取各个路标的大地坐标
std::vector<double> aux; // Auxiliar vector
std::stringstream sname;
for (int i=0; i<m_iLandMarkNum; i++)
{
sname.str("");
sname.clear();
sname<<"LM"<<i;
LM_cfgFile.read_vector("LandMark", sname.str(), aux, aux, false);
m_pLandMarkPose[i].x = aux.at(0);
m_pLandMarkPose[i].y = aux.at(1);
m_pLandMarkPose[i].phi = wrapToPi( aux.at(2) );
}
}
// 创建路径规划线程
m_bThreadStopFlag = false;
//m_hThreadHandle = CreateThread(NULL,NULL,
// (LPTHREAD_START_ROUTINE)PathThread,this,NULL,NULL);
m_hThreadHandle = (HANDLE)_beginthread(PathThread, 0, this);
m_bAgentStarted = true;
cout<<"[+] PathAgent: Started !"<<endl;
return 0;
}
/***************************************************************
【函数功能】： 查找中间用于视觉校正的路径点
【输 入】： path : 完整路径队列
calibPoints : 输出找到的校正点
bFound : 输出是否找到
【输 出】： 无
【说 明】： 查找范围限定在机器人坐标系前方1m点为中心，0.8m*0.4m的矩形ROI区域
！！！试验过程中，发现路标前停止位置偏远，因此减少中心点距离为0.85米！！！
***************************************************************/
void CPathAgent::FindCalibPoses(std::deque<math::TPoint2D> &path,
std::deque<math::TPose2D> &calibPoses, bool &bFound)
{
std::deque<SPose2LM> dequePose2LM;
//std::deque<int> idLands;
calibPoses.clear();
bFound = false;
if(path.size() < 6) return; // 避免路径终点被分割成小段了
// 遍历路径点，判断在哪个路径点能看到路标
for (unsigned int i=5; i<(path.size()-5); i++) // 路径【头5点】和【尾5点】不参与。保证不会出现路径起末点小路径的情况
{
TPoint2D p = path.at(i); // 本次的点
TPoint2D n = path.at(i+1); // 后一个点
TPose2D p2n( n - p ); // p点到n点的矢量
if (abs(p2n.x) < 0.000001) // x==0
{
p2n.phi = (p2n.y > 0) ? (M_PI/2) : (-M_PI/2);
}
else
{
float k = p2n.y / p2n.x;
p2n.phi = (p2n.x > 0) ? atan(k) : (atan(k) + M_PI);
}
p2n.phi = wrapToPi(p2n.phi); // 机器人在该路径点的理想朝向角
CPose2D robot(p.x, p.y, p2n.phi);// 此点上机器人的大地位姿
CPose2D center2robot(m_dCalibCenterDistance, 0, 0); // 查找范围的中心点在机器人坐标系内的坐标
CPose2D center = robot + center2robot; // 查找范围的中心点在大地坐标系内的坐标
// 遍历路标，找到距离0.4米内的路标
for (int j=0; j<m_iLandMarkNum; j++)
{
TPose2D Land = m_pLandMarkPose[j]; // 路标的大地坐标
////////////// 换算到视野中心点坐标系内判断 ////////////////
CPose2D LMpose(Land);
CPose2D Land2Center = LMpose - center; // 路标在视野中心点坐标系内的坐标
CPose2D LandCen(0.05, 0.05, 0); // 路标帖的中心在路标自己坐标系内的坐标
CPose2D LMC2ViewCenter = Land2Center + LandCen; // 路标贴中心在视野中心坐标系内的坐标
if (abs(LMC2ViewCenter.x()) < abs(m_dCalibCenterDistance-0.7)) // 前后范围符合
{
if ( ((LMC2ViewCenter.x() >= 0) && (abs(LMC2ViewCenter.y()) < 0.5))
|| ((LMC2ViewCenter.x() < 0) && (abs(LMC2ViewCenter.y()) < 0.4)) )
{
// 先记下这个点，这个路标。下面再一个路标选一个最近的路径点
double ddis = LMC2ViewCenter.distance2DTo(0,0);
SPose2LM tpose2LM(robot.x(), robot.y(), robot.phi(), ddis, j);
dequePose2LM.push_back(tpose2LM);
}
}
///////////////// 换算判断结束 ////////////////////////
//double dis = center.distance2DTo(Land.x, Land.y);
//if (dis <= 0.4) // 0.4m半径的圆范围
//{
// // 这个路标点满足基本的距离条件，需要进一步判断是否在矩形ROI区域
// CPoint2D Land2Center(Land.x - center.x(), Land.y - center.y()); // 路标在center坐标系的坐标
// CPose2D TurnPose(0,0,-center.phi()); // 旋转到机器人朝向的center坐标系
// CPoint2D newLand2Center = TurnPose + Land2Center;
// if (abs(newLand2Center.x()) < 0.2) // 在矩形ROI区域
// {
// // 记下这个点，这个路标。一个路标选一个最近的路径点
// SPose2LM tpose2LM(robot.x(), robot.y(), robot.phi(), j);
// dequePose2LM.push_back(tpose2LM);
// }
//}
}
}
if (dequePose2LM.size() <= 0) // 没找到校正点
{
bFound = false;
return;
}
else
bFound = true;
// 每个路标只取一个最近校正点
TPose2D minPose;
double minDis = 10;
for (unsigned int i=0; i<dequePose2LM.size(); i++)
{
TPose2D tpose(dequePose2LM.at(i).x, dequePose2LM.at(i).y, dequePose2LM.at(i).phi);
int indexLM = dequePose2LM.at(i).index;
//CPose2D robot(tpose);
//CPose2D center2robot(m_dCalibCenterDistance, 0, 0); // 查找范围的中心点在机器人坐标系内的坐标
//CPose2D center = robot + center2robot; // 查找范围的中心点在大地坐标系内的坐标
//double dis = center.distance2DTo(m_pLandMarkPose[indexLM].x, m_pLandMarkPose[indexLM].y);
double dis = dequePose2LM.at(i).dis;
if (dis < minDis)
{
minDis = dis;
minPose = tpose;
}
if((i+1) < dequePose2LM.size())
{
if (dequePose2LM.at(i+1).index != indexLM) // 这个路标挑完了
{
calibPoses.push_back(minPose);
minDis = 10;
}
}
}
calibPoses.push_back(minPose); // 最后一个校正点
}
void encodersCallback(int32_t left_ticks, int32_t right_ticks) {
ros::Time now = ros::Time::now();
double delta_time = (now - prev_time).toSec();
prev_time = now;
pre_l_encoder=l_encoder;
pre_r_encoder=r_encoder;
l_encoder = left_ticks;
r_encoder = right_ticks;
////velocity from driver encoder readings
double l_tics_per_s=(double)(l_encoder-pre_l_encoder)/delta_time;
double r_tics_per_s=(double)(r_encoder-pre_r_encoder)/delta_time;
double l_w = l_tics_per_s * 2.0 * M_PI / (double) encoder_cpr;
double r_w = r_tics_per_s * 2.0 * M_PI / (double) encoder_cpr;
double l_v = l_w * wheel_diameter / 2.0;
double r_v = r_w * wheel_diameter / 2.0;
double v = (l_v + r_v) / 2.0;
double w = (r_v - l_v) / wheel_base_length;
if (first_enc_read) {
xx = 0;
yy = 0;
tt = 0;
first_enc_read = false;
}
tt = tt + w * delta_time*slip_factor;
tt = wrapToPi(tt);
xx = xx + (v * cos(tt))*delta_time;
yy = yy + (v * sin(tt))*delta_time;
nav_msgs::Odometry odom_msg;
geometry_msgs::Quaternion q_odom;
odom_msg.header.stamp = now;
odom_msg.header.frame_id = odom_frame_id;
odom_msg.pose.pose.position.x = xx;
odom_msg.pose.pose.position.y = yy;
odom_msg.pose.pose.position.z = 0;
if ((fuse_imu_roll_pitch)&&(fuse_imu_yaw)) {
odom_msg.pose.pose.orientation = q_odom=q_imu;
}
else if ((!fuse_imu_roll_pitch)&&(fuse_imu_yaw)) {
double roll = 0, pitch = 0, yaw = 0;
tf::Quaternion q;
tf::quaternionMsgToTF(q_imu, q);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
q_odom =tf::createQuaternionMsgFromRollPitchYaw(0,0, yaw);
}
else if ((fuse_imu_roll_pitch)&&(!fuse_imu_yaw)) {
double roll = 0, pitch = 0, yaw = 0;
tf::Quaternion q;
tf::quaternionMsgToTF(q_imu, q);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
q_odom =tf::createQuaternionMsgFromRollPitchYaw(roll,pitch, tt);
}
else if ((!fuse_imu_roll_pitch)&&(!fuse_imu_yaw)) {
q_odom =tf::createQuaternionMsgFromRollPitchYaw(0,0, tt);
}
odom_msg.pose.pose.orientation=q_odom;
odom_msg.child_frame_id = base_frame_id;
odom_msg.twist.twist.linear.x = v;
odom_msg.twist.twist.linear.y = 0;
odom_msg.twist.twist.angular.z = w;
odom_msg.pose.covariance[0] = pos_cov;
odom_msg.pose.covariance[7] = pos_cov;
odom_msg.pose.covariance[14] = 1e100;
odom_msg.pose.covariance[21] = 1e100;
odom_msg.pose.covariance[28] = 1e100;
odom_msg.pose.covariance[35] = rot_cov;
odom_msg.twist.covariance = odom_msg.pose.covariance;
odom_pub.publish(odom_msg);
//Add TF broadcaster
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = now;
odom_trans.header.frame_id = odom_frame_id;
odom_trans.child_frame_id = base_frame_id;
odom_trans.transform.translation.x = xx;
odom_trans.transform.translation.y = yy;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = q_odom;
odom_broadcaster->sendTransform(odom_trans);
jointstate_msg.header.stamp = ros::Time::now();
jointstate_msg.position[0]=r_w*delta_time;
jointstate_msg.velocity[0]=r_w;
jointstate_msg.position[1]=l_w*delta_time;
jointstate_msg.velocity[1]=l_w;
jointstate_msg.position[2]=r_w*delta_time;
jointstate_msg.velocity[2]=r_w;
jointstate_msg.position[3]=l_w*delta_time;
jointstate_msg.velocity[3]=l_w;
if (have_pan_tilt) {
jointstate_msg.position[5]=pan;
jointstate_msg.position[6]=tilt;
}
pan_tilt_pub.publish(jointstate_msg);
}
inline void wrapToPiInPlace(T &a)
{
a = wrapToPi(a);
}
