TASK
这篇教程主要是测量机器人的里程计误差的,让机器人重复的走固定的路线,在这里我让机器人走正方形,比较机器人的起始位置(理想状态是重合的) 以及 局部里程计(机器人自身测算得)和全局里程计的的位置(定位系统)。
仿真用的机器人里程计是准确的,因此这次实验要在自己的机器上进行。
Algorithm
DeadReckoning(航迹推算) 算法在 NodeDeadReckoningI 类中实现,在你发送指令之前,你需要知道机器人在局部里程计和全局里程计的位置。两个位置都知道后才能运动。在 NodeDeadReckoning 类里面需要需要两个变量 (globalOdomReceived and localOdomReceived)来判断是否接收到位置信息。
通过PID控制机器人直走多少米,转弯多少度,PID控制的反馈数据来源私有程计(local odometry:机器人自带).
机器人静止后,再次等待local和global的里程计并计算与起始点的误差。
输入处理
输入的是odometry消息。你只需要位置和航向角。坐标x,y存放在pose.pose.position.x,pose.pose.position.y,航向角( φ )和四元数z元素之间的关系如下:
方形路径跟随
当收到local和global的里程计数据后,调用commander()函数控制机器人。让机器人跑几圈后,等待接收里程计数据。‘
评估结果
如果global和local的数据都受到了调用evaluateResults()函数,起始点的误差需要计算x、y、
φ
的偏差。local里程计会相对于global里程计飘移。将local的初始位置和global的设定为一致。
θ
记作local和global里程计的初始位置角度偏差
将系统1(local 里程计)坐标变换到系统0(global 里程计)下:
展开:
Program
创建包:
catkin_create_pkg dem_dead_reckoning roscpp rospy std_msgs
跳转到新产生的目录dem_dead_reckoning 下面,创建文件:
- include/node_deadReckoningI.h
- src/node_deadReckoningI.cpp
添加主程序:
- src/deadReckoningI.cpp
为了独立使用PID控制器,把上一节的内容拷贝到合理的文件夹下面:
- src/myPoint.cpp
- src/node_pid.cpp
- src/pid.cpp
- include/myPoint.h
- include/node_pid.h
修改CMakeLists.txt,添加下面内容 :
set (SRCS1 ${SRCS1} src/myPoint.cpp)
set (SRCS1 ${SRCS1} src/node_pid.cpp)
set (SRCS1 ${SRCS1} src/pid.cpp)
set (SRCS2 ${SRCS2} src/node_deadReckoningI.cpp)
set (SRCS2 ${SRCS2} src/deadReckoningI.cpp)
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(cmd ${SRCS1})
target_link_libraries(cmd ${catkin_LIBRARIES})
add_executable(deadReckoningReg ${SRCS2})
target_link_libraries(deadReckoningReg ${catkin_LIBRARIES})
Code: node_deadReckoningI.h
#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include <string>
using namespace std;
class NodeDeadReckoningI
{
public:
/* Constructor
* pub Publisher, which can send commands to robot.
* fw Variable, which will be stored in forward.
* an Variable, which will be stored in angle.
* num Variable, which will be stored in numberOfTrips.
*/
NodeDeadReckoningI(int num, double fw, double an);
~NodeDeadReckoningI();
/* This method receives global odometry data.
* If #evaluate = false, than this method processes
* start position.
* If #evaluate = true, than this method processes
* end position and start method evaluateResults()
*
* msg Message with global odometry data
*/
void messageCallback(const nav_msgs::Odometry::ConstPtr& msg);
/* This method is similar to messageCallback,
* but receives local odometry data.
*
* msg Message with local odometry data.
*/
void messageCallback2(const nav_msgs::Odometry::ConstPtr& msg);
/* This method controls movement of robot (Trips forward
* and turn around by using PID controller).
*/
void commander();
/* Evaluate difference between start and end
* position in global odometry and compare
* global with local odometry.
*/
void evaluateResults();
//variables
double forward; // Distance to go forward
double angle; // Angle to turn around
int numberOfTrips; // How many times will robot go forward and turn.
//Initial position (Loc - local odometry)
geometry_msgs::Point start;
double startAngle;
geometry_msgs::Point startLoc;
double startLocAngle;
//Final position (Loc - local odometry)
geometry_msgs::Point end;
double endAngle;
geometry_msgs::Point endLoc;
double endLocAngle;
bool evaluate; // True if robot finished its trajectory
bool localOdomReceived; // True, if local odometry is received
bool globalOdomReceived; // True, if global odometry is received
};
Code: node_deadReckoningI.cpp
#include "node_deadReckoningI.h"
#include <cstdlib>
#define PI 3.141592654
//Constructor and destructor
NodeDeadReckoningI::NodeDeadReckoningI(int num, double fw, double an)
{
numberOfTrips = num;
forward = fw;
angle = an;
}
NodeDeadReckoningI::~NodeDeadReckoningI()
{
}
//Subscriber
void NodeDeadReckoningI::messageCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
if (globalOdomReceived == false && evaluate == false)
{
globalOdomReceived = true;
start = msg->pose.pose.position;
startAngle = 2.0*asin(msg->pose.pose.orientation.z);
if (localOdomReceived == true)
{
commander();
}
}
else if (evaluate)
{
globalOdomReceived = true;
end = msg->pose.pose.position;
endAngle = 2.0*asin(msg->pose.pose.orientation.z);
if (localOdomReceived == true)
{
evaluateResults();
}
}
}
//Subscriber
void NodeDeadReckoningI::messageCallback2(const nav_msgs::Odometry::ConstPtr& msg)
{
if (localOdomReceived == false && evaluate == false)
{
localOdomReceived = true;
startLoc = msg->pose.pose.position;
startLocAngle = 2.0*asin(msg->pose.pose.orientation.z);
if (globalOdomReceived == true)
{
commander();
}
}
else if (evaluate)
{
localOdomReceived = true;
endLoc = msg->pose.pose.position;
endLocAngle = 2.0*asin(msg->pose.pose.orientation.z);
if (globalOdomReceived == true)
{
evaluateResults();
}
}
}
void NodeDeadReckoningI::commander(){
stringstream sf;
sf << "./cmd -f " << forward;
stringstream sr;
sr << "./cmd -r " << angle;
for (int tripsDone = 0; tripsDone < numberOfTrips ; tripsDone++)
{
std::system(sf.str().c_str());
std::system(sr.str().c_str());
}
evaluate = true;
localOdomReceived = false;
globalOdomReceived = false;
}
void NodeDeadReckoningI::evaluateResults(){
//evaluating results
double diffx = end.x - start.x;
double diffy = end.y - start.y;
double diffAngle = endAngle - startAngle;
//TRANSFORMATION
double angleTransStart = startAngle - startLocAngle; //How much is local angle different from global
double xStartLocNew = startLoc.x*cos(angleTransStart)-startLoc.y*sin(angleTransStart);
double yStartLocNew = startLoc.x*sin(angleTransStart)+startLoc.y*cos(angleTransStart);
double xEndLocNew = endLoc.x*cos(angleTransStart)-endLoc.y*sin(angleTransStart);
double yEndLocNew = endLoc.x*sin(angleTransStart)+endLoc.y*cos(angleTransStart);
double diffxLoc = xEndLocNew - xStartLocNew ;
double diffyLoc = yEndLocNew - yStartLocNew ;
double diffAngleLoc = endLocAngle - startLocAngle;
while (fabs(diffAngle) > PI)
{
diffAngle -= copysign(2*PI,diffAngle);
}
//sending message
ROS_INFO("DIFFERENCE BETWEEN LOCAL AND GLOBAL ODOMETRY:\nDIFFERENCES IN AXIS:\nDIFF_X = %f\nDIFF_Y = %f\nDIFF_ANGLE = %f",diffxLoc-diffx,diffyLoc-diffy,diffAngleLoc-diffAngle);
ROS_INFO("DIFFERENCE FROM START POSITION:\nDIFERENCES IN AXIS:\nDIFF_X = %f\nDIFF_Y = %f\nDIFF_ANGLE = %f",diffx, diffy, diffAngle);
exit(0);
}
Code: deadReckoningI.cpp
#include "node_deadReckoningI.h"
#define SUBSCRIBER_BUFFER_SIZE 1 // Size of buffer for subscriber.
#define NUMBER_OF_TRIPS 8 // How many times will robot go forward and turn
#define FORWARD 0.6 // Distance to go forward.
#define ANGLE 1.570796 // Robot will turn around by this angle
#define SUBSCRIBER_TOPIC "/syros/global_odom"
#define SUBSCRIBER2_TOPIC "/syros/base_odom"
int main(int argc, char **argv)
{
//Initialization of node
ros::init(argc, argv, "deadReckoningI");
ros::NodeHandle n;
//Creating object, which stores data from sensors and has methods for subscribing
NodeDeadReckoningI *nodeDeadReckoningI = new NodeDeadReckoningI(NUMBER_OF_TRIPS, FORWARD, ANGLE);
//Creating subscribers
ros::Subscriber sub = n.subscribe(SUBSCRIBER_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodeDeadReckoningI::messageCallback, nodeDeadReckoningI);
ros::Subscriber sub2 = n.subscribe(SUBSCRIBER2_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodeDeadReckoningI::messageCallback2, nodeDeadReckoningI);
ros::spin();
return 0;
}
编译
catkin_make
运行程序:
rosrun dem_dead_reckoning deadReckoningReg
译者注:粘贴代码时候一定注意在 .h文件里面添加防止重编译的宏定义
代码github地址: https://github.com/will1991/rosintrodution