从开环到闭环的旅程-CoCube
差动驱动机器人轨迹-CoCube
迷宫逃离的问题-CoCube
自由运动和环境限制-CoCube
001，自由运动
002，引出环境
003，对比差异
ROS机器人从起点到终点（四）蓝桥云实践复现
cocube自由运动
机器人也需要一个目标，没有目标就没有方向感。
有了目标，如何从起点运动到终点呢？误差如何呢？
这时候就需要算法啦。
tutorials/move.cpp
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"
#include "math.h"
#include
ros::Subscriber sub;
ros::Publisher pub;
float goal_x = 2;
float goal_y = 2;
void sendVel(const turtlesim::Pose::ConstPtr& data){
ros::NodeHandle n;
pub = n.advertise("/turtle1/cmd_vel",100);
float curr_x = data->x;
float curr_y = data->y;
float curr_ang = data->theta;
float dist = sqrt(pow(goal_x-curr_x,2) + pow(goal_y-curr_y,2));
std::cout << "Distance = " << dist << std::endl;
if(dist > 0.01){
double ang = atan2((float)(goal_y-curr_y),(float)(goal_x-curr_x));
std::cout << "Curr_ang = " << curr_ang << " | ang = " << ang << std::endl;
geometry_msgs::Twist t_msg;
t_msg.linear.x = 1.0*(dist);
t_msg.angular.z = 4.0*(ang-curr_ang);
pub.publish(t_msg);
}
else
{
std::cout << "Mission Completed" << std::endl;
std::cout << "Please enter new coordinates" << std::endl;
std::cout << "Please enter goal_x:" << std::endl;
std::cin >> goal_x;
std::cout << "Please enter goal_y:" << std::endl;
std::cin >> goal_y;
}
}
int main(int argc, char **argv){
ros::init(argc,argv,"goToGoal");
ros::NodeHandle n;
sub = n.subscribe("/turtle1/pose",100,sendVel);
ros::spin();
return 0;
} 这里，有一个bug，后续解决，问题在一个突变点-pi和pi这个点，当然不止这一个bug。
然后修改CMakelist：
add_executable(move tutorials/move.cpp)
target_link_libraries(move ${catkin_LIBRARIES})
add_dependencies(move turtlesim_gencpp)
install(TARGETS turtlesim_node turtle_teleop_key draw_square mimic move
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})然后运行即可。
Distance = 0.0793549
Curr_ang = 0.617521 | ang = 0.617525
Distance = 0.0780842
Curr_ang = 0.617522 | ang = 0.617518
Distance = 0.0768352
Curr_ang = 0.617521 | ang = 0.617522
Distance = 0.0756069
Curr_ang = 0.617521 | ang = 0.617519
Distance = 0.0743976
Curr_ang = 0.617521 | ang = 0.617523
Distance = 0.0732063
Curr_ang = 0.617521 | ang = 0.617512
Distance = 0.0720351
Curr_ang = 0.617521 | ang = 0.617528
Distance = 0.0708819
Curr_ang = 0.617521 | ang = 0.617531
Distance = 0.0697493
Curr_ang = 0.617522 | ang = 0.617525
Distance = 0.0686332
Curr_ang = 0.617522 | ang = 0.61752
Distance = 0.0675334
Curr_ang = 0.617522 | ang = 0.617515
Distance = 0.0664527
Curr_ang = 0.617521 | ang = 0.617517
Distance = 0.0653911
Curr_ang = 0.617521 | ang = 0.617527
Distance = 0.0643448
Curr_ang = 0.617522 | ang = 0.617513
Distance = 0.0633133
Curr_ang = 0.617521 | ang = 0.617518
Distance = 0.0623009
Curr_ang = 0.617521 | ang = 0.61753
Distance = 0.0613038
Curr_ang = 0.617521 | ang = 0.617519
Distance = 0.0603242
Curr_ang = 0.617521 | ang = 0.617533
Distance = 0.0593584
Curr_ang = 0.617522 | ang = 0.617541
Distance = 0.0584078
Curr_ang = 0.617523 | ang = 0.617524
Distance = 0.0574748
Curr_ang = 0.617523 | ang = 0.617534
Distance = 0.0565544
Curr_ang = 0.617524 | ang = 0.617509
Distance = 0.05565
Curr_ang = 0.617523 | ang = 0.617532
Distance = 0.0547598
Curr_ang = 0.617524 | ang = 0.6175
Distance = 0.0538829
Curr_ang = 0.617522 | ang = 0.617509
Distance = 0.0530208
Curr_ang = 0.617521 | ang = 0.61754
Distance = 0.0521729
Curr_ang = 0.617522 | ang = 0.617513
Distance = 0.0513383
Curr_ang = 0.617522 | ang = 0.617529
Distance = 0.0505164
Curr_ang = 0.617522 | ang = 0.617506
Distance = 0.0497078
Curr_ang = 0.617521 | ang = 0.617529
Distance = 0.0489129
Curr_ang = 0.617522 | ang = 0.617543
Distance = 0.0481306
Curr_ang = 0.617523 | ang = 0.617518
Distance = 0.0473606
Curr_ang = 0.617523 | ang = 0.617506
Distance = 0.0466027
Curr_ang = 0.617522 | ang = 0.617509
Distance = 0.0458571
Curr_ang = 0.617521 | ang = 0.617527
Distance = 0.0451225
Curr_ang = 0.617521 | ang = 0.617527
Distance = 0.0444017
Curr_ang = 0.617522 | ang = 0.617518
Distance = 0.0436904
Curr_ang = 0.617521 | ang = 0.617514
Distance = 0.0429913
Curr_ang = 0.617521 | ang = 0.617526
Distance = 0.0423032
Curr_ang = 0.617521 | ang = 0.617519
Distance = 0.0416274
Curr_ang = 0.617521 | ang = 0.617528
Distance = 0.0409611
Curr_ang = 0.617522 | ang = 0.617543
Distance = 0.0403059
Curr_ang = 0.617523 | ang = 0.617537
Distance = 0.0396603
Curr_ang = 0.617524 | ang = 0.617538
Distance = 0.0390257
Curr_ang = 0.617525 | ang = 0.617517
Distance = 0.0384018
Curr_ang = 0.617524 | ang = 0.617542
Distance = 0.0377878
Curr_ang = 0.617525 | ang = 0.617503
Distance = 0.0371829
Curr_ang = 0.617524 | ang = 0.617542
Distance = 0.0365881
Curr_ang = 0.617525 | ang = 0.617516
Distance = 0.0360027
Curr_ang = 0.617525 | ang = 0.617497
Distance = 0.0354253
Curr_ang = 0.617523 | ang = 0.617514
Distance = 0.034859
Curr_ang = 0.617522 | ang = 0.617508
Distance = 0.0343023
Curr_ang = 0.617521 | ang = 0.617509
Distance = 0.0337524
Curr_ang = 0.617521 | ang = 0.617504
Distance = 0.0332121
Curr_ang = 0.617519 | ang = 0.617506
Distance = 0.0326812
Curr_ang = 0.617519 | ang = 0.617515
Distance = 0.0321589
Curr_ang = 0.617518 | ang = 0.617485
Distance = 0.0316445
Curr_ang = 0.617516 | ang = 0.617496
Distance = 0.031138
Curr_ang = 0.617515 | ang = 0.617551
Distance = 0.0306389
Curr_ang = 0.617517 | ang = 0.617514
Distance = 0.0301494
Curr_ang = 0.617517 | ang = 0.617484
Distance = 0.0296662
Curr_ang = 0.617515 | ang = 0.617536
Distance = 0.0291931
Curr_ang = 0.617516 | ang = 0.617508
Distance = 0.0287252
Curr_ang = 0.617516 | ang = 0.617509
Distance = 0.0282654
Curr_ang = 0.617515 | ang = 0.617559
Distance = 0.0278128
Curr_ang = 0.617518 | ang = 0.617507
Distance = 0.0273683
Curr_ang = 0.617517 | ang = 0.617503
Distance = 0.0269306
Curr_ang = 0.617516 | ang = 0.617491
Distance = 0.0265009
Curr_ang = 0.617515 | ang = 0.61753
Distance = 0.0260754
Curr_ang = 0.617516 | ang = 0.617545
Distance = 0.0256583
Curr_ang = 0.617518 | ang = 0.61751
Distance = 0.0252477
Curr_ang = 0.617517 | ang = 0.617571
Distance = 0.0248444
Curr_ang = 0.617521 | ang = 0.617519
Distance = 0.0244464
Curr_ang = 0.617521 | ang = 0.617501
Distance = 0.0240552
Curr_ang = 0.617519 | ang = 0.617475
Distance = 0.0236705
Curr_ang = 0.617517 | ang = 0.617552
Distance = 0.0232916
Curr_ang = 0.617519 | ang = 0.617555
Distance = 0.0229196
Curr_ang = 0.617521 | ang = 0.61755
Distance = 0.0225517
Curr_ang = 0.617523 | ang = 0.617516
Distance = 0.0221918
Curr_ang = 0.617523 | ang = 0.617542
Distance = 0.0218362
Curr_ang = 0.617524 | ang = 0.617538
Distance = 0.0214874
Curr_ang = 0.617525 | ang = 0.617525
Distance = 0.0211429
Curr_ang = 0.617525 | ang = 0.617481
Distance = 0.0208047
Curr_ang = 0.617522 | ang = 0.617553
Distance = 0.0204724
Curr_ang = 0.617524 | ang = 0.617542
Distance = 0.0201442
Curr_ang = 0.617525 | ang = 0.617498
Distance = 0.0198214
Curr_ang = 0.617523 | ang = 0.617498
Distance = 0.0195054
Curr_ang = 0.617522 | ang = 0.617487
Distance = 0.0191921
Curr_ang = 0.617519 | ang = 0.6175
Distance = 0.0188857
Curr_ang = 0.617518 | ang = 0.617502
Distance = 0.0185835
Curr_ang = 0.617517 | ang = 0.617469
Distance = 0.0182866
Curr_ang = 0.617514 | ang = 0.617485
Distance = 0.0179939
Curr_ang = 0.617512 | ang = 0.617465
Distance = 0.0177066
Curr_ang = 0.617509 | ang = 0.617495
Distance = 0.0174219
Curr_ang = 0.617508 | ang = 0.617551
Distance = 0.0171429
Curr_ang = 0.617511 | ang = 0.617508
Distance = 0.0168693
Curr_ang = 0.617511 | ang = 0.617516
Distance = 0.0165999
Curr_ang = 0.617511 | ang = 0.617485
Distance = 0.0163343
Curr_ang = 0.617509 | ang = 0.617575
Distance = 0.0160718
Curr_ang = 0.617514 | ang = 0.617532
Distance = 0.0158161
Curr_ang = 0.617515 | ang = 0.617474
Distance = 0.0155616
Curr_ang = 0.617512 | ang = 0.617514
Distance = 0.0153139
Curr_ang = 0.617512 | ang = 0.617542
Distance = 0.0150678
Curr_ang = 0.617514 | ang = 0.617498
Distance = 0.014827
Curr_ang = 0.617513 | ang = 0.617513
Distance = 0.0145904
Curr_ang = 0.617513 | ang = 0.617484
Distance = 0.0143565
Curr_ang = 0.617511 | ang = 0.617485
Distance = 0.0141279
Curr_ang = 0.61751 | ang = 0.61755
Distance = 0.0139009
Curr_ang = 0.617512 | ang = 0.617538
Distance = 0.013678
Curr_ang = 0.617514 | ang = 0.617478
Distance = 0.013459
Curr_ang = 0.617512 | ang = 0.617564
Distance = 0.0132441
Curr_ang = 0.617515 | ang = 0.617604
Distance = 0.0130324
Curr_ang = 0.617521 | ang = 0.617476
Distance = 0.0128244
Curr_ang = 0.617518 | ang = 0.6175
Distance = 0.0126191
Curr_ang = 0.617517 | ang = 0.617561
Distance = 0.0124169
Curr_ang = 0.617519 | ang = 0.617446
Distance = 0.0122184
Curr_ang = 0.617515 | ang = 0.617491
Distance = 0.0120226
Curr_ang = 0.617513 | ang = 0.617575
Distance = 0.0118299
Curr_ang = 0.617517 | ang = 0.617476
Distance = 0.011641
Curr_ang = 0.617515 | ang = 0.617545
Distance = 0.0114537
Curr_ang = 0.617517 | ang = 0.61752
Distance = 0.0112716
Curr_ang = 0.617517 | ang = 0.617574
Distance = 0.0110912
Curr_ang = 0.61752 | ang = 0.617531
Distance = 0.0109133
Curr_ang = 0.617521 | ang = 0.617527
Distance = 0.0107397
Curr_ang = 0.617521 | ang = 0.617462
Distance = 0.0105672
Curr_ang = 0.617518 | ang = 0.617543
Distance = 0.0103989
Curr_ang = 0.617519 | ang = 0.617563
Distance = 0.0102322
Curr_ang = 0.617522 | ang = 0.617476
Distance = 0.0100681
Curr_ang = 0.617519 | ang = 0.617431
Distance = 0.00990781
Mission Completed
Please enter new coordinates
Please enter goal_x:
基于MSRDS机器人仿真平台的多机器人PID编队控制算法
bug点解决的办法如上这篇。
简单试一试看能不能复现出bug。
不能达到目标点，高速暴冲！！！复现1
cocube稳定到失控
不能达到目标点，高速振荡！！！复现2
cocube稳定到振荡
不用再复现了，就是个～垃～圾～代码啊……
就这么一个简单的小程序，就能出现如此严重的bug。
测试是非常重要的，并且不可缺少！
创建文件move.cpp（或想要的任何名称）并将其粘贴到源目录中。
了解代码
TurtleBot类
TurtleBot类将包含机器人的所有方面，例如位置姿态、发布者和订阅者、订阅者回调函数和“移动到目标”函数。
订阅者
订阅者将订阅主题“/turtle1/pose”，这是发布实际机器人位置的主题。当收到消息时调用函数update_pose，并将实际位置保存在名为pose的类属性中。
欧氏位置法
该方法将使用先前保存的海龟位置（即自身位置姿势）和参数（即数据）来计算海龟和目标之间的点对点（欧几里得）距离。
比例控制器
为了让机器人移动，将对线速度和角速度使用比例控制。线性速度将由常数乘以机器人和目标位置之间的距离组成，角速度将取决于y轴距离的反正切乘以x轴距离乘以常数。
误差容忍度
必须在=目标点周围创建一个公差区，因为精确达到目标需要非常高的精度。在这段代码中，如果使用一个非常小的精度，海龟会发疯（你可以试试！）。换句话说，代码和模拟器被简化了，所以它不能以完全精确的方式工作。
参考案例Python PID：
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import pow,atan2,sqrt
PI = 3.1415926535897
# PID parameters for rotation
p_r = 3
i_r = 0.00001
d_r = 1
# PID parameters for translation
p_t = 1.3
i_t = 0.0001
d_t = 1
class turtlebot():
def __init__(self):
#Creating our node,publisher and subscriber
rospy.init_node('turtlebot_controller', anonymous=True)
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, self.callback)
self.pose = Pose()
self.rate = rospy.Rate(10)
#Callback function implementing the pose value received
def callback(self, data):
self.pose = data
self.pose.x = round(self.pose.x, 4)
self.pose.y = round(self.pose.y, 4)
def get_distance(self, goal_x, goal_y):
distance = sqrt(pow((goal_x - self.pose.x), 2) + pow((goal_y - self.pose.y), 2))
return distance
def move2goal(self):
goal_pose = Pose()
goal_pose.x = input("Set your x goal:")
goal_pose.y = input("Set your y goal:")
goal_theta = input("Set your theta goal:")
distance_tolerance = input("Set your distance tolerance:")
theta_tol = input("Set your theta tolerance:")
while goal_theta > 360:
goal_theta = goal_theta - 360
# conerting angles from degrees to radians
goal_pose.theta = goal_theta * PI / 180
theta_tolerance = theta_tol * PI / 180
vel_msg = Twist()
#PID Controller
#rotate delta_1
e_theta1_old = 0
ei_theta1 = 0
while abs(atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.theta) >= theta_tolerance:
en_theta1 = atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.theta
ed_theta1 = en_theta1 - e_theta1_old
ei_theta1 = ei_theta1 + en_theta1
#linear velocity
vel_msg.linear.x = 0
vel_msg.linear.y = 0
vel_msg.linear.z = 0
#angular velocity in the z-axis:
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = p_r * en_theta1 + i_r * ei_theta1 + d_r * ed_theta1
#Publishing our vel_msg
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
e_theta1_old = en_theta1
#Stopping our robot after the movement is over
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
#delta translate
e_translatn_old = 0
ei_translatn = 0
while sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2)) >= distance_tolerance:
en_translatn = sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2))
ed_translatn = en_translatn - e_translatn_old
ei_translatn = ei_translatn + en_translatn
#linear velocity in the x-axis:
vel_msg.linear.x = p_t * en_translatn + i_t * ei_translatn + d_t * ed_translatn
vel_msg.linear.y = 0
vel_msg.linear.z = 0
#angular velocity in the z-axis:
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 0
#Publishing our vel_msg
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
e_translatn_old = en_translatn
#Stopping our robot after the movement is over
vel_msg.linear.x = 0
self.velocity_publisher.publish(vel_msg)
#delta rotation 2
e_theta2_old = 0
ei_theta2 = 0
while abs(goal_pose.theta - self.pose.theta) >= theta_tolerance:
en_theta2 = goal_pose.theta - self.pose.theta
ed_theta2 = en_theta2 - e_theta2_old
ei_theta2 = ei_theta2 + en_theta2
#linear velocity
vel_msg.linear.x = 0
vel_msg.linear.y = 0
vel_msg.linear.z = 0
#angular velocity in the z-axis:
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = p_r * en_theta2 + i_r * ei_theta2 + d_r * ed_theta2
#Publishing our vel_msg
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
e_theta2_old = en_theta2
#Stopping our robot after the movement is over
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
rospy.spin()
if __name__ == '__main__':
try:
#Testing our function
x = turtlebot()
x.move2goal()
except rospy.ROSInterruptException: pass