03-ros1-robot_move

Ros ros lidar imu

发布时间 : 2025-03-10 09:52

字数:1.8k 阅读 :

机器人的运动

矢量运动
- x 前后
- y 左右
- z 上下
旋转运动
- x轴滚转
- y轴俯仰
- z轴自转

如何确认坐标轴

坐标系确认的方法 右手定则

如何确认旋转正方向

怎么确定旋转的正方向 右手螺旋定则

这个方向便是正方向

速度量纲

矢量运动

量纲为 m/s

旋转运动

量纲为 弧度/s

pi = 3.14,,, = 180°

消息包便是这样的

ROS机器人运动控制

上述的节点约定俗称,

/cmd_ve l 运动状态消息节点是约定的写法

仿真

没有使用机器人使用仿真软件来做

项目的的地址:

https://github.com/6-robot/wpr_simulation

启动仿真软件

roslaunch wpr_simulation wpb_simple.launch

启动后画面呈现这样

启动运动控制实例程序

rosrun wpr_simulation demo_vel_ctrl

运行完之后，这个机器人会一直往前前进

实现思路

创建在catkin_ws/src下创建 pkg

caktin_create_pkg vel_pkg roscpp rospy geometry_msgs

C++实现

vel_node.cpp

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

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

    ros::NodeHandle n;
    ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);  // 修正拼写错误

    geometry_msgs::Twist vel_msg;  // 修正拼写错误
    vel_msg.linear.x = 0.1;
    vel_msg.linear.y = 0;
    vel_msg.linear.z = 0;
    vel_msg.angular.x = 0;
    vel_msg.angular.y = 0;
    vel_msg.angular.z = 10;

    ros::Rate r(30);

    while (ros::ok()) {
        vel_pub.publish(vel_msg);
        r.sleep();
    }   

    return 0;
}

直接旋转起来了+往前前进

Python实现

#!/usr/bin/env python3
# coding=utf-8

import rospy
from geometry_msgs.msg import Twist

if __name__ == "__main__":
    rospy.init_node("demo_vel_ctrl")
    # 发布速度控制话题
    vel_pub = rospy.Publisher("cmd_vel",Twist,queue_size=10)
    # 构建速度消息包并赋值
    vel_msg = Twist()
    vel_msg.linear.x = 0.1
    # 构建发送频率对象
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        vel_pub.publish(vel_msg)
        rate.sleep()

激光雷达的工作原理

雷达的种类

TOF激光雷达

激光雷达的消息包格式

雷达主题的 topic

/scan

查看数据包

rostopic echo /scan --noarr

–noarr 不显示数组
查看的数据如下

这是雷达的消息格式

intensities

angle_min 起始角度
angel_max 扫描中止角度
time_increment
scan_time 扫描一次花费的时间[扫描频率]
range_min 扫描的最小距离
range_max 扫描的最大距离
[]ranges
- 有360个值，表示从起始角度到中止角度的各个角度
- INF 表示超出范围了，就没有值
intensities 表示激光雷达的强度
- 360，对应每个电的强度信息

使用 `RViz` 观察传感器数据

使用之前的

roslaunch wpr_simulation wpb_simple.launch

打开 RViz

rviz

`rviz` 界面的操作

rviz是数据观测程序，只是可视化程序

选择Fixed Framee —> base_footprint

添加模型 RobotModel
添加雷达 LaserScan
- 话题选择 /scan

`Gazebo` 仿真程序

获取雷达的数据

消息格式
- sensor_msgs
话题名称
- /scan

C++实现

创建 lidar_ppkg

catkin_create_pkg lidar_pkg rospy roscpp sensor_msgs

创建

lidar_node.cpp

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>


void LidarCallback(const sensor_msgs::LaserScan msg)
{
    float fMidDist = msg.ranges[180];
    ROS_INFO("前方测距 ranges[180] = %f 米",fMidDist);
}



int main(int argc,char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"lidar_node");

    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan",10,&LidarCallback);
    ros::spin();
    return 0;
}

编译运行

Python实现

lidar_node.py

#!/usr/bin/env python3
# coding=utf-8

import rospy    
from sensor_msgs.msg import LaserScan

def lidar_callback(msg:LaserScan):
    dist = msg.ranges[180]
    rospy.loginfo(f"前方测距 ranges[180] = {dist} 米")


def main():
    rospy.init_node("lidar_node")
    lidar_sub = rospy.Subscriber("/scan",LaserScan,lidar_callback,queue_size=10)
    rospy.spin()


if __name__ == "__main__":
    main()

记得 赋予执行权限 然后运行

实现雷达运动避障

C++实现

前方避障

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>

ros::Publisher vel_pub;
int nCount =0;

void LidarCallback(const sensor_msgs::LaserScan msg)
{
    float fMidDist = msg.ranges[180];
    ROS_INFO("前方测距 ranges[180] = %f 米",fMidDist); // c++ 14
    // ROS_INFO_STREAM("前方测距 ranges[180] = " << fMidDist << " 米");
    
    if (nCount >0)
    {
        nCount --;
        return;
    }


    geometry_msgs::Twist vel_cmd;
    if (fMidDist < 1.5)
    {
        vel_cmd.angular.z = 0.3; // trun left
        nCount = 50;
    }
    else
    {
        vel_cmd.linear.x = 0.05; // foward
    }
    vel_pub.publish(vel_cmd);

}



int main(int argc,char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"lidar_node");
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan",10,&LidarCallback);

    // publisher
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);

    ros::spin();
    return 0;
}

Python实现

IMU介绍

IMU是 惯性测量单元消息包

sensor_msgs/ImuMessage

angular_velocity —> 角速度
linear_acceleration —> 矢量加速度
orientation 姿态信息数据
- 使用四元数描述法，为了解决 欧拉角 死锁问题

获取IMU数据

IMU发布的 话题

imu
- data_raw 输入
- data 融合后的四元组姿态描述
- mag 只有9轴机器人才有

C++实现

29节

创建 imu_pkg 节点

catkin_craete_pkg imu_pkg rospy roscpp sensor_msgs

imu_node.cpp

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>

void IMUCallback(sensor_msgs::Imu msg)
{
    if(msg.orientation_covariance[0]<0)
        return;
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );

    double roll,pitch,yaw;
    tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw); // rad/s
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("滚转=%.0f 俯仰=%.0f 朝向=%.0f",roll,pitch,yaw);
}


int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"imu_node");

    ros::NodeHandle n;
    ros::Subscriber imu_sub = n.subscribe("/imu/data",4,IMUCallback);

    ros::spin();
    return 0;
}

CMakeLists.txt

add_executable(imu_node src/imu_node.cpp)
add_dependencies(imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(imu_node
  ${catkin_LIBRARIES}
)

启动仿真程序，启动运行

Python实现

imu_node.py

#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import math


def imu_callback(msg:Imu):
    if msg.orientation_covariance[0] <0 : # not exist
        return
    
    quaternion =[
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w,
    ]

    data = euler_from_quaternion(quaternion)
    data = [i*180/math.pi for i in data]
    roll,pitch,yaw = data
    rospy.loginfo("滚转=%.0f 俯仰=%.0f 朝向=%.0f",roll,pitch,yaw)


def main():
    rospy.init_node("imu_node")
    imu_sub = rospy.Subscriber("/imu/data",Imu,imu_callback,queue_size=3)

    rospy.spin()

if __name__=="__main__":
    main()

启动仿真环境，启动脚本

IMU航向锁定

C++实现

让机器人可以始终保持一个方向

imu_node.cpp

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>
#include <geometry_msgs/Twist.h>

ros::Publisher vel_pub;

void IMUCallback(sensor_msgs::Imu msg)
{
    if(msg.orientation_covariance[0]<0)
        return;
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );

    double roll,pitch,yaw;
    tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw); // rad/s
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("滚转=%.0f 俯仰=%.0f 朝向=%.0f",roll,pitch,yaw);

    double target_yaw = 90;
    double diff_angle = target_yaw -yaw;

    geometry_msgs::Twist vel_cmd;
    vel_cmd.angular.z = diff_angle*0.01;
    vel_pub.publish(vel_cmd);


}


int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"imu_node");

    ros::NodeHandle n;
    ros::Subscriber imu_sub = n.subscribe("/imu/data",4,IMUCallback);

    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);


    ros::spin();
    return 0;
}

python实现

imu_node.py

#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Twist
import math



vel_pub = None

def imu_callback(msg:Imu):
    if msg.orientation_covariance[0] <0 : # not exist
        return
    
    quaternion =[
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w,
    ]

    data = euler_from_quaternion(quaternion)
    data = [i*180/math.pi for i in data]
    roll,pitch,yaw = data
    rospy.loginfo("滚转=%.0f 俯仰=%.0f 朝向=%.0f",roll,pitch,yaw)

    target_yaw = 180
    diff_angle = target_yaw - yaw
    vel_cmd = Twist()
    vel_cmd.angular.z = diff_angle * 0.01
    vel_cmd.linear.x = 0.1
    vel_pub.publish(vel_cmd)

def main():
    global vel_pub
    rospy.init_node("imu_node")
    imu_sub = rospy.Subscriber("/imu/data",Imu,imu_callback,queue_size=10)
    vel_pub = rospy.Publisher("/cmd_vel",Twist,queue_size=10)
    rospy.spin()

if __name__=="__main__":
    main()

03-ros1-robot_move

机器人的运动

如何确认坐标轴

如何确认旋转正方向

速度量纲

矢量运动

旋转运动

ROS机器人运动控制

仿真

实现思路

C++实现

Python实现

激光雷达的工作原理

雷达的种类

激光雷达的消息包格式

使用 RViz 观察传感器数据

rviz 界面的操作

Gazebo 仿真程序

获取雷达的数据

C++实现

Python实现

实现雷达运动避障

C++实现

Python实现

IMU介绍

获取IMU数据

C++实现

Python实现

IMU航向锁定

C++实现

python实现

使用 `RViz` 观察传感器数据

`rviz` 界面的操作

`Gazebo` 仿真程序