ros小车轨迹控制(个人纪录）

命令行发送cmd_vel指令

1.让小车直行

rostopic pub /cmd_vel geometry_msgs/Twist "linear:   
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" 

小车沿着x轴运动
2. 让小车转圈

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 2.0" 

小车做圆周运动

新建一个节点不断发送/cmd_vel控制小车运动

odometry1.cpp文件

#include <iostream>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <ros/console.h>
#include<unistd.h>

using namespace std;
int main(int argc,char** argv)
{
    ros::init(argc, argv, "cmdveltest");
     ros::NodeHandle cmdh;
    ros::Publisher cmdpub= cmdh.advertise<geometry_msgs::Twist>("/cmd_vel", 1, true);;
    ros::Rate r(60);
    while(1){
        geometry_msgs::Twist twist;
        geometry_msgs::Vector3 linear;
        linear.x=0.1;
        linear.y=0;
        linear.z=0;
        geometry_msgs::Vector3 angular;
        angular.x=0;
        angular.y=0;
        //直行
        //angular.z=0;
        //转圈
        angular.z=-0.5;
        twist.linear=linear;
        twist.angular=angular;

        cmdpub.publish(twist);
        cout<<"hello"<<endl;
       // ros::spinOnce();
        //r.sleep();
        sleep(1);
    }
    return 0;
}

在CMakeLists.txt
加入

add_executable(odometry1 src/odometry1.cpp)
target_link_libraries(odometry1 ${catkin_LIBRARIES})

rosrun yuan odometry1 小车做圆形运动

控制小车到达指定地点

dingdian.py文件

#!/usr/bin/python3
import rospy
import math
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion

x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0
 
X_t = 0
Y_t = 0
X_sim = cos(angle)      
Y_sim = sin(angle)     
 
 
def Trans_robot_pose(msg): 
    
    global x
    global y
    global w_o   
    global x_o
    global y_o
    global z_o
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    w_o = msg.pose.pose.orientation.w
    x_o = msg.pose.pose.orientation.x
    y_o = msg.pose.pose.orientation.y
    z_o = msg.pose.pose.orientation.z
    return w_o, y_o, z_o, x_o, x, y
 
 
if __name__ == '__main__':
    rospy.init_node('item1')
 
    turtle_vel = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
    rate = rospy.Rate(10.0)
 
    while not rospy.is_shutdown():
        msg = geometry_msgs.msg.Twist()
        (roll, pitch, yaw) = euler_from_quaternion([x_o,y_o,z_o,w_o])  
        if yaw < 0:
            yaw = yaw + 2 * math.pi
 
        X_t = X_sim
        Y_t = Y_sim
 
        D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))
       
        if (Y_t - y) == 0 and (X_t - x) > 0:
            yaw_t = 0
        if (Y_t - y) > 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x))
        if (Y_t - y) > 0 and (X_t - x) == 0:
            yaw_t = 0.5 * math.pi
        if (Y_t - y) > 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) == 0 and (X_t - x) < 0:
            yaw_t = math.pi
        if (Y_t - y) < 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) < 0 and (X_t - x) == 0:
            yaw_t = 1.5 * math.pi
        if (Y_t - y) < 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi
 
        Theta_err = yaw_t - yaw
        if Theta_err < -math.pi:
            Theta_err = Theta_err + 2 * math.pi
        if Theta_err > math.pi:
            Theta_err = Theta_err - 2 * math.pi
     
        liner_speed = 0.1 * D_err
        angular_speed = 0.7 * Theta_err
 
        msg.linear.x = liner_speed
        msg.angular.z = angular_speed
 
        liner_speed_old = liner_speed
        angular_speed_old = angular_speed
        turtle_vel.publish(msg)   
        rospy.Subscriber('/odom', nav_msgs.msg.Odometry,  Trans_robot_pose) 
 
        rate.sleep()
    rospy.spin()

在创建功能包时 需要包含文件中需要的所有依赖 否则编译时报错

rosrun yuan dingdian.py

小车会运动到指定地点

小车沿着给定轨迹运动

guiji.py文件

#!/usr/bin/python3
import rospy
import math
import nav_msgs.msg
import geometry_msgs.msg
from tf.transformations import euler_from_quaternion

x = 0
y = 0
w_o = 0
x_o = 0
y_o = 0
z_o = 0
yaw_t = 0
liner_speed = 0
angular_speed = 0
liner_speed_old = 0
angular_speed_old = 0
 
X_t = 0
Y_t = 0
X_t_Pre = 0
Y_t_Pre = 0
X_sim = [5,  1, 2.5,  4, 0,   0,   5,  5,  0, 0]        
Y_sim = [0, -4, 2.5, -4, 0, 2.5, 2.5, -4, -4, 0]       
r = 0
 
 
def Trans_robot_pose(msg): 
    global x
    global y
    global w_o   
    global x_o
    global y_o
    global z_o
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    w_o = msg.pose.pose.orientation.w
    x_o = msg.pose.pose.orientation.x
    y_o = msg.pose.pose.orientation.y
    z_o = msg.pose.pose.orientation.z
    return w_o, y_o, z_o, x_o, x, y
 
 
if __name__ == '__main__':
    rospy.init_node('item1')
 
    turtle_vel = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
    rate = rospy.Rate(10.0)
 
    while not rospy.is_shutdown():
        msg = geometry_msgs.msg.Twist()
        (roll, pitch, yaw) = euler_from_quaternion([x_o,y_o,z_o,w_o])  
        if yaw < 0:
            yaw = yaw + 2 * math.pi
 
        X_t = X_sim[r]
        Y_t = Y_sim[r]
 
        D_err = math.sqrt(math.pow((X_t - x), 2) + math.pow((Y_t - y), 2))

        if (Y_t - y) == 0 and (X_t - x) > 0:
            yaw_t = 0
        if (Y_t - y) > 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x))
        if (Y_t - y) > 0 and (X_t - x) == 0:
            yaw_t = 0.5 * math.pi
        if (Y_t - y) > 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) == 0 and (X_t - x) < 0:
            yaw_t = math.pi
        if (Y_t - y) < 0 and (X_t - x) < 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + math.pi
        if (Y_t - y) < 0 and (X_t - x) == 0:
            yaw_t = 1.5 * math.pi
        if (Y_t - y) < 0 and (X_t - x) > 0:
            yaw_t = math.atan((Y_t - y) / (X_t - x)) + 2 * math.pi
 
        Theta_err = yaw_t - yaw
 
        if Theta_err < -math.pi:
            Theta_err = Theta_err + 2 * math.pi
        if Theta_err > math.pi:
            Theta_err = Theta_err - 2 * math.pi
 
        if D_err < 0.3:       
            X_t_Pre = X_t
            Y_t_Pre = Y_t
            r = r + 1
            print r
            if r == 10:
                r = 0
           
        liner_speed = 0.1 * D_err
        angular_speed = 0.7 * Theta_err
 
        msg.linear.x = liner_speed
        msg.angular.z = angular_speed
 
        liner_speed_old = liner_speed
        angular_speed_old = angular_speed
        turtle_vel.publish(msg)  
        rospy.Subscriber('/odom', nav_msgs.msg.Odometry,  Trans_robot_pose)
 
        rate.sleep()
    rospy.spin()

rosrun yuan guiji.py

小车沿着轨迹运动

如何用rviz查看小车轨迹

发布nav_msgs/Path类型的消息，然后在rviz上订阅该消息就可以显示轨迹路径
showpath.cpp

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <cstdlib>

main(int argc, char **argv)
{
    ros::init(argc, argv, "showpath");

    ros::NodeHandle ph;
    ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("trajectory", 1, true);
    ros::Publisher point_pub = ph.advertise<geometry_msgs::PointStamped>("point", 1, true);

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    nav_msgs::Path path;

    //nav_msgs::Path path;
    path.header.stamp = current_time;
    path.header.frame_id = "odom";

    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    double th = 0.0;
    double vx = 0.1 + 0.2 * rand() / double(RAND_MAX);
    double vy = -0.1 + 0.2 * rand() / double(RAND_MAX);
    double vth = 0.1;

    ros::Rate loop_rate(10);
    while (ros::ok())
    {

        current_time = ros::Time::now();
        //compute odometry in a typical way given the velocities of the robot
        double dt = 1;
        double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
        double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
        double delta_th = vth * dt;

        x += delta_x;
        y += delta_y;
        z += 0.005;
        th += delta_th;

        geometry_msgs::PoseStamped this_pose_stamped;
        geometry_msgs::PointStamped this_point_stamped;

        this_pose_stamped.pose.position.x = x;
        this_pose_stamped.pose.position.y = y;
        this_pose_stamped.pose.position.z = z;

        this_point_stamped.header.stamp = current_time;
        this_point_stamped.header.frame_id = "odom";
        this_point_stamped.point.x = x;
        this_point_stamped.point.y = y;
        this_point_stamped.point.z = z;
        ROS_INFO("current_x: %f", x);
        ROS_INFO("current_y: %f", y);
        geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);
        this_pose_stamped.pose.orientation.x = goal_quat.x;
        this_pose_stamped.pose.orientation.y = goal_quat.y;
        this_pose_stamped.pose.orientation.z = goal_quat.z;
        this_pose_stamped.pose.orientation.w = goal_quat.w;

        this_pose_stamped.header.stamp = current_time;
        this_pose_stamped.header.frame_id = "odom";
        path.poses.push_back(this_pose_stamped);

        path_pub.publish(path);
        point_pub.publish(this_point_stamped);
        ros::spinOnce(); // check for incoming messages

        last_time = current_time;
        loop_rate.sleep();
    }

    return 0;
}

运行showpath.cpp
打开rviz
add path
修改frame_id的odom


问题？
启动gazebo控制小车运动 rviz中没有轨迹显示

rviz中警告

和查询tf_tree时的报错类似

两个小车跟随

小车发送位置信息需要在模型中添加插件

<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
    	<alwaysOn>true</alwaysOn>
    	<updateRate>50.0</updateRate>
    	<bodyName>base_link</bodyName>
   	<topicName>base_pose_ground_truth</topicName>
    	<gaussianNoise>0.01</gaussianNoise>
    	<frameName>world</frameName>
    	<xyzOffsets>0 0 0</xyzOffsets>
   	<rpyOffsets>0 0 0</rpyOffsets>
 </plugin>

监听广播器的launch文件

 <launch>
    <node pkg="learning_tf" type="tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
    <node pkg="learning_tf" type="slave_tf_listener" name="listener" />
  </launch>

运行监听广播器后报错

启动键盘控制节点

 ROS_NAMESPACE=turtle1 rosrun teleop_twist_keyboard teleop_twist_keyboard.py

控制小车1 小车2 不动