最近在ROS ANSWERS中看到这样一个问题:
Hi, I’m a beginner and I’m struggling to make my robot move to a certain point. How should I set the angular and linear velocity for my robot to move to a certain point ? For example, if my robot initial position is (0,0), what angular and linear velocity should I use for it to go to (5,5) ?
其实,这位同学所说的就是机器人导航,也就是让机器人从一个地方移动到另一个地方。 当然,为了实现这个过程,你需要考虑至少以下三件事情:- 坐标(0,0)和(5,5)是在哪个坐标系下?
- 你需要设置障碍物躲避吗(也就是在你的机器人移动到目标位置的时候,躲避那些可能出现在(0,0)和(5,5)之间的障碍物)?
- 机器人移动的位置是已知还是未知?
- 我们是在里程计坐标系下进行的坐标计算(假设你知道什么是里程计)。里程计坐标系存在一个问题,随着时间的推移会有漂移,但对于这位同学提到的问题已经够用了。
- 我们路径上没有设置障碍物。因此,我们不需要采用势场或其它避障算法。
- 假设这是一个未知的环境,我们不会在该环境中构建地图。
STEP 0. 在ROS下创建工程
STEP 1.移动机器人
首先,我们创建工程名为my_robot的package,运行以下命令: 为了将机器人从A移动到B,我们需要使用一个控制器。我们在my_robot的src中创建my_robot.cpp文件,核心代码如下:double inc_x = goal.x -x;double inc_y = goal.y -y;double angle_to_goal = atan2(inc_y, inc_x);if abs(angle_to_goal - theta) > 0.1{ speed.linear.x = 0.0; speed.angular.z = 0.3;}else{ speed.linear.x = 0.5 speed.angular.z = 0.0}
在my_robot目录下新建launch文件夹,在launch文件夹下新建my_robot.launch文件,注意在robot.world中创建一个目标点:
<launch><node name = "stage" pkg="stage_ros" type = "stageros" args="$(find stage_ros)/world/robot.world"/><node name = "my_robot" pkg = "my_robot" type = "my_robot"output = "screen" />launch>
运行:
cd ~/robot_ws //跳转到工作空间目录catkin_makeroslaunch my_robot my_robot.launch
结果显示:
可以看到,通过比对设置的目标点和里程计之间的差异,机器人最终到了我们设置的目标点。附上本文完整源代码:
#include #include #include #include #include #include #include #include using namespace std;double x = 0.0;double y = 0.0;double theta = 0.0;double rot_q = 0.0;double roll = 0.0;double pitch = 0.0;void NewOdom(const nav_msgs::Odometry::ConstPtr& msg){x = msg->pose.pose.position.x;y = msg->pose.pose.position.y;rot_q = msg->pose.pose.orientation;(roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w]);}int main(int argc, char **argv){ros::init(argc, argv, "speed_controller");ros::NodeHandle n;//create a handle noderos::Subscriber sub = n.subscribe("/odometry/filtered", &NewOdom, this);ros::Publisher pub = n.advertise<:twist>("/cmd_vel", 1);ros::Rate loop_rate(10);geometry_msgs::Twist speed;geometry_msgs::Point goal;goal.x = 5;goal.y = 5;while(ros::ok()){double inc_x = goal.x -x;double inc_y = goal.y -y;double angle_to_goal = atan2(inc_y, inc_x);if (abs(angle_to_goal - theta) > 0.1){speed.linear.x = 0.0;speed.angular.z = 0.3;}else{speed.linear.x = 0.5;speed.angular.z = 0.0;}pub.publish(speed);ros::spinOnce();}return 0;}
古月居原创作者签约计划已开启,网站(guyuehome.com)已上线【投稿】功能,欢迎大家积极投稿,原创优质文章作者将有机会成为古月居签约作者。
MoveIt编程技巧
课程限时特价中
扫码查看→