ROS学习零碎记录

科幻木影

已于 2025-02-24 19:44:00 修改

阅读量920

点赞数 24

CC 4.0 BY-SA版权

文章标签： python 机器人 linux 计算机视觉 c++

于 2024-11-11 07:01:56 首次发布

本文链接：https://blog.csdn.net/CCS_Jack/article/details/143671052

vscode插件安装：C/C++、CMake、IntelliCode、Markdown All in One、Msg Language Support、Python、ROS、URDF、vscode-icons（来源：古月居）

ros和vscode的安装：鱼香ros一键安装

wget http://fishros.com/install -O fishros && . fishros

ros快捷键Ctrl+Shift+B编译，并设置默认编译器：按下Ctrl+Shfit+P，输入指令tasks:configure task，选择catkin_make: build，将task.json里的group值改为{"kind":"build","isDefault":true}
ModuleNotFoundError: No module named 'cv_bridge'：sudo apt-get install ros-<对应的ros版本名称>-cv-bridge
编译完后，需在终端中重新source一下，否则运行的还是旧程序
如果编译报过错，将程序撤回到能正常编译的状态，但此时也可能会编译失败，可将build文件夹删除重新编译一次

创建工作空间和功能包

# 创建工作空间 ros_ws为自定义工作空间名称
mkdir -p ~/ros_ws/src

# 切换到工作空间的src
cd ~/ros_ws/src

# 创建功能包 ros_pkg为自定义功能包名称 rospy和roscpp为依赖
catkin_create_pkg ros_pkg rospy roscpp

# 切换到工作空间
cd ~/ros_ws

# 编译
catkin_make

关于 ~/.bashrc 中添加多个工作空间，可能会出现 source 的顺序错乱，工作空间被覆盖的问题

1.问题说明：

你有没有遇到这种情况？明明已经将工作空间中的 setup.bash 添加进 ~/.bashrc 了，可是终端却找不到对应的功能包，需要在终端中再 source 一下才能找到

2.原因：

没有按照先后创建并编译工作空间的顺序添加 setup.bash 到 ~/.bashrc

3.如何避免：

养成习惯，每次添加 setup.bash 到 ~/.bashrc 前，都把 build 和 devel 文件夹删了，然后重新编译一次，且在添加时只能在末尾添加，不要试图添加到以前添加的 source 命令前

4.解决方法：

方法1：按照 ~/.bashrc 中 source 工作空间的顺序，将对应 build 和 devel 文件夹删了并重新编译；

方法2：找到 ~/.bashrc 中，最后一个被 source 的工作空间，在其 devel/_setup_util.py 中，找到CMAKE_PREFIX_PATH变量，按需将其他工作空间的 devel 添加进去即可。

功能包下，Python文件一般放自建的scripts里（其实随便命名一个文件夹也行），而Cpp文件则放在src里，头文件则放在include里

功能包下的Python文件需要给予可执行权限才能用rosrun执行

# 命令行方式
sudo chmod +x 你的Python文件.py

# 文件夹右键属性方式

Bash一行命令自动执行多输入

# EOF是End Of File的缩写，表示自定义终止符。既然自定义，那么EOF就不是固定的，可以随意设置别名，意思是把内容当作标准输入传给程序，Linux中按Ctrl-d就代表EOF

# 通常将EOF与 << 结合使用，表示后续的输入作为子命令或子Shell的输入，直到遇到EOF为止，再返回到主调Shell

# 使用示例
cat <<EOF
<1
<2
<3
<EOF

多相机同时采集和显示

import cv2
import numpy as np
import time
import multiprocessing
import threading
import os
import shutil

    
def cameraProcess(id, key_q:multiprocessing.Queue):
    def show(id, width, height, resize):
        nonlocal is_running
        nonlocal frame
        nonlocal timestamp
        nonlocal key_ctl

        file_path = os.path.dirname(__file__)

        if os.name == "posix":
            dir = os.path.join(file_path, f"cam/cam{id}")
        elif os.name == "nt":
            dir = os.path.join(file_path, f"cam\\cam{id}")

        if not os.path.exists(dir):
            os.makedirs(dir)
        else:
            shutil.rmtree(dir)
            os.makedirs(dir)

        while is_running:
            if frame is None:
                continue
            img = frame.copy()
            frame = None
            raw_10 = img.view(dtype=np.int16)
            raw_sh = np.right_shift(raw_10, 2)
            raw_8 = raw_sh.astype(np.uint8)
            img = raw_8.reshape(height, width)
            img = cv2.cvtColor(img, cv2.COLOR_BAYER_BG2GRAY)

            if resize:
                img_show = cv2.resize(img, (640, 480))
                img_show = cv2.flip(img_show, -1)
                cv2.imshow(f"Video{id}", img_show)
            else:
                cv2.imshow(f"Video{id}", img)

            key = cv2.waitKey(1)
            if key == 27 or key_ctl == "exit":
                is_running = False
            elif key_ctl == "save":
                path = os.path.join(dir, str(timestamp) + ".png")
                cv2.imwrite(path, img)
            key_ctl = None
            
        cv2.destroyAllWindows()

    def getFrame(cam):
        nonlocal is_running
        nonlocal frame
        nonlocal timestamp

        while is_running and cam.isOpened():
            timestamp = int(time.time() * 1e9)
            ret, frame = cam.read()
            if not ret:
                is_running = False
        cam.release()

    cam = cv2.VideoCapture(id)
    width = int(cam.get(cv2.CAP_PROP_FRAME_WIDTH))
    height = int(cam.get(cv2.CAP_PROP_FRAME_HEIGHT))
    cam.set(cv2.CAP_PROP_CONVERT_RGB, 0)

    print(width, height)

    resize = False
    if width != 640:
        resize = True
    
    is_running = True
    frame = None
    timestamp = 0
    key_ctl = None

    getFrame_thread = threading.Thread(target=getFrame, args=(cam,), daemon=True)
    show_thread = threading.Thread(target=show, args=(id, width, height, resize,), daemon=True)
    
    getFrame_thread.start()
    show_thread.start()

    while is_running:
        time.sleep(0.01)
        try:
            key_ctl = key_q.get(block=False)
        except:
            continue

    getFrame_thread.join()
    show_thread.join()


if __name__=="__main__":
    key_q1 = multiprocessing.Queue()
    key_q2 = multiprocessing.Queue()

    cam1 = multiprocessing.Process(target=cameraProcess, args=(0, key_q1,), daemon=True)
    cam2 = multiprocessing.Process(target=cameraProcess, args=(1, key_q2,), daemon=True)

    cam1.start()
    cam2.start()

    mat = np.zeros((320, 320))

    cv2.namedWindow("key_control", 1)
    cv2.resizeWindow("key_control", 320, 320)

    while True:
        cv2.imshow("key_control", mat)
        key = cv2.waitKey(1)
        if key == 27:
            key_q1.put("exit")
            key_q2.put("exit")
            break
        elif key == ord('s'):
            key_q1.put("save")
            key_q2.put("save")
    cv2.destroyWindow("key_control")

    cam1.join()
    cam2.join()

发布USB相机话题示例

#!/usr/bin/env python3
#coding:utf-8
 
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
 
def pubVideo():
    rospy.init_node('play_node',anonymous = False)
    pub = rospy.Publisher('/camera/image', Image, queue_size = 10)
    # rate = rospy.Rate(100)
    bridge = CvBridge()
    cap = cv2.VideoCapture(0)
    width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
    height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
    cap.set(cv2.CAP_PROP_CONVERT_RGB, 0)
    if not cap.isOpened():return -1
    while not rospy.is_shutdown():
        # rate.sleep()
        now = rospy.Time.now()
        ret, frame = cap.read()
        if ret:
            raw_10 = frame.view(dtype=np.int16)
            raw_sh = np.right_shift(raw_10, 2)
            raw_8 = raw_sh.astype(np.uint8)
            img = raw_8.reshape(height, width)
            # img2 = cv2.cvtColor(img, cv2.COLOR_BAYER_BG2RGB)
            img2 = cv2.cvtColor(img, cv2.COLOR_BAYER_BG2GRAY)
            msg = bridge.cv2_to_imgmsg(img2)
            msg.header.stamp = now
            rospy.loginfo("%.6f"%msg.header.stamp.to_sec())
            pub.publish(msg)
    cap.release()
       
if __name__ == '__main__':
    pubVideo()

命令行传参发布相机或视频话题

#!/usr/bin/env python3
#coding:utf-8
 
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import argparse
import subprocess
 
def pubVideo(id=0):
    rospy.init_node('camera_node',anonymous = False)
    pub = rospy.Publisher(f'/image', Image, queue_size = 10)

    if args.rate:
        rate = rospy.Rate(args.rate)
    bridge = CvBridge()

    cap = cv2.VideoCapture(id)
    width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
    height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))

    if not cap.isOpened():
        return -1

    while not rospy.is_shutdown():
        if args.rate:rate.sleep()
        now = rospy.Time.now()
        ret, frame = cap.read()
        if ret:
            msg = bridge.cv2_to_imgmsg(frame)
            msg.header.stamp = now
            msg.width = width
            msg.height = height
            rospy.loginfo("%.6f"%msg.header.stamp.to_sec())
            pub.publish(msg)
    cap.release()
       
if __name__ == '__main__':
    p = argparse.ArgumentParser(description="相机或视频话题发布")
    p.add_argument("-i", "--input", type=str, help="相机id或视频路经")
    p.add_argument("-s", "--start", type=int, default=0, help="开始遍历的相机id")
    p.add_argument("-r", "--rate", type=int, help="读取的速度hz")

    args = p.parse_args()

    if args.input:
        path = args.input
        try:
            path = int(path)
        except:
            pass
        pubVideo(path)
        exit()
    
    res = subprocess.run("ls /dev/video*", stdout=subprocess.PIPE, shell=True).stdout.decode().strip().split('\n')

    if len(res) == 1 and res[0] == '':
        print("Not find camera")
        exit()

    for i in range(args.start, len(res)):
        id = int(res[i][-1])
        cap = cv2.VideoCapture(id)
        if not cap.isOpened():
            cap.release()
            continue
        cap.release()
        print("cam_id:", int(res[i][-1]))
        res = pubVideo(id)
        break

订阅相机话题示例

#!/usr/bin/env python3
#coding:utf-8
 
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

bridge = CvBridge()

def subVideo(msg):
    img = bridge.imgmsg_to_cv2(msg)
    rospy.loginfo("%.6f"%msg.header.stamp.to_sec())
    cv2.imshow("img", img)
    cv2.waitKey(1)
       
if __name__ == '__main__':
    rospy.init_node('show_node')
    sub = rospy.Subscriber('/camera/image', Image, subVideo)
    rospy.spin()

相机视场角计算

import cv2
import numpy as np

def camera_fov(K: np.ndarray, D: np.ndarray, distortion_mode: int = 0):
    """
    计算相机的视场角
    
    :param K 相机内参3*3
    :param D 相机畸变参数5*1
    :param distortion_mode 畸变模型, 0：鱼眼模型，1：径向切向模型
    :return: 相机的视场角1*3，水平, 垂直, 对角
    """
    cx = K[0, 2]
    cy = K[1, 2]
    p1 = (0, cy)
    p2 = (cx, 0)
    p3 = (0, 0)
    if distortion_mode == 0:
        D1 = D[0:4] if type(D) == np.ndarray else 0
        # 输入像素坐标，输出归一化坐标
        pds = cv2.fisheye.undistortPoints(np.array([[p1, p2, p3]]), K, D1)
        w = np.abs(pds[0][0][0])
        h = np.abs(pds[0][1][1])
        d = np.sqrt(pds[0][2][0]**2 + pds[0][2][1]**2)   
    else:
        # 输入像素坐标，输出归一化坐标
        pds = cv2.undistortPoints(np.array([p1, p2, p3]), K, D)
        w = np.abs(pds[0][0][0])
        h = np.abs(pds[1][0][1])
        d = np.sqrt(pds[2][0][0]**2 + pds[2][0][1]**2)
    ax = 2 * np.degrees(np.arctan(w))
    ay = 2 * np.degrees(np.arctan(h))
    ad = 2 * np.degrees(np.arctan(d))
    return ax, ay, ad


if __name__ == "__main__":

    K = np.array([
            [1.4160544575705535e+03, 0, 3.195e+02],
            [0, 1.4160544575705535e+03, 2.395e+02],
            [0, 0, 1]
        ])

    D = np.array([
            [-9.4068857788802127e-02],
            [2.9141590071782497],
            [0.0],
            [0.0],
            [-1.3025885590073166e+01]
        ])

    print(camera_fov(K, D, 1))

多话题消息同步后发布话题，同步后的频率向订阅的话题中频率最低的看齐

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

import rospy
import message_filters
from sensor_msgs.msg import Image, Imu

def multi_callback(Sub_img1, Sub_img2, Sub_imu):
    pub_img1.publish(Sub_img1)
    pub_img2.publish(Sub_img2)
    pub_imu.publish(Sub_imu)

if __name__ == '__main__':
    rospy.init_node('together_node', anonymous=False) #初始化节点

    sub_img1 = message_filters.Subscriber('/cam0/image', Image)
    sub_img2 = message_filters.Subscriber('/cam1/image', Image)
    sub_imu = message_filters.Subscriber('/imu/imu', Imu) 

    pub_img1 = rospy.Publisher('/cam0/image_t', Image, queue_size = 10)
    pub_img2 = rospy.Publisher('/cam1/image_t', Image, queue_size = 10)
    pub_imu = rospy.Publisher('/imu/imu_t', Imu, queue_size = 10)
 
    sync = message_filters.ApproximateTimeSynchronizer([sub_img1, sub_img2, sub_imu], 10, 1) #同步时间戳，slop为可以同步的最大延迟（以秒为单位）
    sync.registerCallback(multi_callback)   #执行反馈函数
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("over!")

kalibr标定工具箱的安装和使用github

1.启动ros内核
roscore

2.查看bag信息
rosbag info cam.bag

3.标定
3.1 设置环境变量
export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1
3.2 开始
rosrun kalibr kalibr_calibrate_cameras --models pinhole-equi --target april_9x9.yaml --bag cam.bag --topics /image
3.3 设置初始焦距
3.4 写成一行
export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1;echo 400 | rosrun kalibr kalibr_calibrate_cameras --models pinhole-equi --target april_9x9.yaml --bag test.bag --topics /cam0/image_raw

4.解包 图片命名十九位数
rosrun kalibr kalibr_bagextractor --image-topics /image --imu-topics /imu --output-folder cam/ --bag cam.bag

5.打包 图片命名十九位数
rosrun kalibr kalibr_bagcreater --folder cam/. --output-bag test.bag
5.1 打包目录示例
-cam
--cam0
---1385030208726607500.png
---i1385030212176607500.png
--cam1
---1385030208726607500.png
---1385030212176607500.png
--imu0.csv
5.2 imu.csv示例
timestamp,omega_x,omega_y,omega_z,alpha_x,alpha_y,alpha_z
1385030208736607488,0.5,-0.2,-0.1,8.1,-1.9,-3.3
 ...
1386030208736607488,0.5,-0.1,-0.1,8.1,-1.9,-3.3

6.标定help
rosrun kalibr kalibr_calibrate_cameras -h

python进程绑定cpu内核的方法

import os

# 绑定倒数十个核
cpu_ave = list(os.sched_getaffinity(os.getpid()))[::-1][:10]
os.sched_setaffinity(os.getpid(), cpu_ave)

rosbag的使用

# 录制/topic_name1和/topic_name2的话题，打包成output.bag
rosbag record /topic_name1 /topic_name2 -O output.bag

# 以0.5倍速播放output.bag
rosbag play output.bag -r 0.5

# 查看output.bag信息
rosbag info output.bag

关于VMware虚拟机ubuntu无网络问题

1.先检查一下VM虚拟网络开机自启服务有没有被关闭

2.如果电脑主机有网络，但虚拟机右上角有线网络图标消失了，则删除NetworkManager缓存文件，重启网络
sudo service NetworkManager stop 
sudo rm /var/lib/NetworkManager/NetworkManager.state 
sudo service NetworkManager start

欧拉角转旋转矩阵与可视化（原始方法）

import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation

# 欧拉角转旋转矩阵，外旋
def euler2matrix(euler, seq='xyz', degrees:bool=True):
    r = Rotation.from_euler(seq, euler, degrees)
    R = r.as_matrix()
    return R

# 画出转移矩阵
def plot_transform_matrix(T, ax, scale, name):
    R = T[0:3, 0:3]
    # 绘制旋转后的坐标系
    new_origin = T[0:3, 3]
    new_x_axis = R[:, 0]
    new_y_axis = R[:, 1]
    new_z_axis = R[:, 2]

    ax.quiver(*new_origin, *new_x_axis, color='r', length=scale, normalize=True)
    ax.quiver(*new_origin, *new_y_axis, color='g', length=scale, normalize=True)
    ax.quiver(*new_origin, *new_z_axis, color='b', length=scale, normalize=True)

    # 设置图例
    ax.legend()
    ax.text(*new_origin, name)

# 右手螺旋定则，从轴的正方向看向原点，逆正顺负
a1 = np.deg2rad(-90)
a2 = np.deg2rad(-60)
a3 = np.deg2rad(-90)

# 绕x轴旋转a1度的旋转矩阵
Rx = np.array([[1, 0, 0],
               [0, np.cos(a1), -np.sin(a1)],
               [0, np.sin(a1), np.cos(a1)]])
# 绕Y轴旋转a2度的旋转矩阵
Ry = np.array([[np.cos(a2), 0, np.sin(a2)],
               [0, 1, 0],
               [-np.sin(a2), 0, np.cos(a2)]])
# 绕Z轴旋转a3度的旋转矩阵
Rz = np.array([[np.cos(a3), -np.sin(a3), 0],
               [np.sin(a3), np.cos(a3), 0],
               [0, 0, 1]])

# 按XYZ顺序，绕旋转后的自身轴旋转(内旋，右乘)，原始系到目标系，欧拉角
R = Rx @ Ry @ Rz

# 按XYZ顺序，绕参考系的固定轴旋转(外旋，左乘)，参考系到目标系，一般以外旋为主
R = Rz @ Ry @ Rx

R = euler2matrix((a1, a2, a3), degrees=False)

# 构造转移矩阵T，旋转矩阵与平移矩阵的组合体，4x4
T = np.zeros((4, 4))
T[0:3,0:3] = R
T[0:3,3] = 0.01
print(T)

fig = plt.figure()
ax = fig.add_subplot(projection='3d')
plot_transform_matrix(np.identity(4), ax, 0.005, 'base')
plot_transform_matrix(T, ax, 0.005, 'rot')

plt.axis('equal')
plt.show()

欧拉角、旋转矩阵、四元数、旋转向量的互换（Python scipy）

import numpy as np
from scipy.spatial.transform import Rotation

'''
一、四元数 (x, y, z, w)
二、旋转向量 (rx, ry, rz) 弧度或角度
三、欧拉角(内旋) 弧度或角度:
    1.绕前后轴Roll 绕左右轴Pitch 绕上下轴Yaw
    2.(yaw-z, pitch-y, roll-x) 世界坐标系 前x左y上z ZYX 常用
    3.(yaw-y, pitch-x, roll-z) 相机坐标系 前z右x下y
    4.欧拉角万向锁问题(奇异性):俯仰角为+-90°时，第一次旋转与第三次旋转将使用同一个轴，使系统丢失了一个自由度

'''

'''旋转矩阵'''

# 旋转矩阵转旋转向量
def matrix2rvec(R, degrees:bool=False):
    r = Rotation.from_matrix(R)
    rvec = r.as_rotvec(degrees)
    return rvec

# 旋转向量转旋转矩阵
def rvec2matrix(rvec, degrees:bool=False):
    r = Rotation.from_rotvec(rvec, degrees)
    R = r.as_matrix()
    return R

# 旋转矩阵转四元数
def matrix2quat(R):
    r = Rotation.from_matrix(R)
    quat = r.as_quat()
    return quat

# 四元数转旋转矩阵
def quat2matrix(quat):
    r = Rotation.from_quat(quat)
    matrix = r.as_matrix()
    return matrix

# 旋转矩阵转欧拉角 euler顺序与seq相同
def matrix2euler(R, seq='xyz', degrees:bool=True):
    r = Rotation.from_matrix(R)
    euler = r.as_euler(seq, degrees)
    return euler

# 欧拉角转旋转矩阵 euler顺序与seq相同
def euler2matrix(euler, seq='xyz', degrees:bool=True):
    r = Rotation.from_euler(seq, euler, degrees)
    R = r.as_matrix()
    return R

'''旋转向量'''

# 旋转向量转四元数
def rvec2quat(rvec, degrees:bool=False):
    r = Rotation.from_rotvec(rvec, degrees)
    quat = r.as_quat()
    return quat

# 四元数转旋转向量
def quat2rvec(quat, degrees:bool=False):
    r = Rotation.from_quat(quat)
    rvec = r.as_rotvec(degrees)
    return rvec

# 旋转向量转欧拉角 euler顺序与seq相同
def rvec2euler(rvec, seq='xyz', degrees_in:bool=False, degrees_out:bool=True):
    r = Rotation.from_rotvec(rvec, degrees_in)
    euler = r.as_euler(seq, degrees_out)
    return euler

# 欧拉角转旋转向量 euler顺序与seq相同
def euler2rvec(euler, seq='xyz', degrees_in:bool=True, degrees_out:bool=False):
    r = Rotation.from_euler(seq, euler, degrees_in)
    rvec = r.as_rotvec(degrees_out)
    return rvec

'''四元数'''

# 四元数转欧拉角 euler顺序与seq相同
def quat2euler(quat, seq='xyz', degrees:bool=True):
    r = Rotation.from_quat(quat)
    euler = r.as_euler(seq, degrees)
    return euler

# 欧拉角转四元数 euler顺序与seq相同
def euler2quat(euler, seq='xyz', degrees:bool=True):
    r = Rotation.from_euler(seq, euler, degrees)
    quat = r.as_quat()
    return quat

'''原始方法 外旋 XYZ'''

def isRotationMatrix(R):
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype=R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6

def rot2euler(R):
    assert (isRotationMatrix(R))

    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    singular = sy < 1e-6

    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2]) * 180 / np.pi
        y = np.arctan2(-R[2, 0], sy) * 180 / np.pi
        z = np.arctan2(R[1, 0], R[0, 0]) * 180 / np.pi
    else:
        x = np.arctan2(-R[1, 2], R[1, 1]) * 180 / np.pi
        y = np.arctan2(-R[2, 0], sy) * 180 / np.pi
        z = 0

    return np.array([x, y, z])

旋转向量与旋转矩阵的互换（opencv）

import cv2

'''
jacobian 雅可比矩阵
'''

rx = -0.0137838
ry = -0.0034871
rz = 3.13607115

rvec = (rx, ry, rz)

# 旋转向量转旋转矩阵
R, jacobian = cv2.Rodrigues(rvec)

# 旋转矩阵转旋转向量
rvec, jacobian = cv2.Rodrigues(R)

雅可比矩阵的计算方法

import sympy as sp

# 定义符号变量
x, y, k1, k2, k3, k4, k5, k6, p1, p2, r = sp.symbols('x y k1 k2 k3 k4 k5 k6 p1 p2 r')

# 定义半径平方
r_squared = x**2 + y**2

kr = (1 + k1 * r_squared + k2 * r_squared**2 + k3 * r_squared**3) / (1 + k4 * r_squared + k5 * r_squared**2 + k6 * r_squared**3)

# 畸变后的坐标
x_distorted = x * kr + 2 * p1 * x * y + p2 * (r_squared + 2 * x**2)
y_distorted = y * kr + p1 * (r_squared + 2 * y**2) + 2 * p2 * x * y

# 创建函数向量
f = sp.Matrix([x_distorted, y_distorted])

# 计算雅可比矩阵
J = f.jacobian([x, y])

values = {
    x: 0.472428,  # 替换 x 的值
    y: -0.126214,  # 替换 y 的值
    k1: -0.093702,  # 替换 k1 的值
    k2: -0.008392,  # 替换 k2 的值
    k3: 0.000,  # 替换 k3 的值
    k4: 0.000,  # 替换 k4 的值
    k5: 0.000,  # 替换 k5 的值
    k6: 0.000,  # 替换 k6 的值
    p1: -0.000315,  # 替换 p1 的值
    p2: 0.000155   # 替换 p2 的值
}

print("雅可比矩阵的每个元素：")
for i in range(J.shape[0]):
    for j in range(J.shape[1]):
        simplified_expr = sp.simplify(J[i, j].subs(x**2 + y**2, r))
        evaluated_expr = J[i, j].subs(values).evalf()
        print(f"J({i}, {j}) = {simplified_expr}\n")
        print(f"J({i}, {j}) = {evaluated_expr}\n")

# 比较元素
print("\n元素间相等性比较结果：")
for i in range(J.shape[0]):
    for j in range(J.shape[1]):
        for k in range(J.shape[0]):
            for l in range(J.shape[1]):
                if (i != k or j != l) and sp.simplify(J[i, j] - J[k, l]) == 0:
                    print(f"J({i}, {j}) 等于 J({k}, {l})")

print("\n雅可比矩阵：")
sp.pprint(J)

旋转向量转欧拉角（opencv，pnp，外旋，绕固定轴，XYZ，相机【z前x右y下】到目标）

R = cv2.Rodrigues(rvec)[0]
proj = np.hstack((R, tvec))
euler = cv2.decomposeProjectionMatrix(proj)[6].reshape(1, -1)[0]

# 按xyz顺序旋转的欧拉角
pitch = euler[0] # x
yaw = euler[1]   # y
roll = euler[2]  # z

坐标系变换之变换矩阵T

import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation

'''
四元数 (x, y, z, w)
旋转向量 (rx, ry, rz)
'''

# 旋转矩阵转四元数 
def matrix2quat(rotation_matrix):
    r = Rotation.from_matrix(rotation_matrix)
    quat = r.as_quat()
    return quat

# 四元数转旋转矩阵
def quat2matrix(quaternion):
    r = Rotation.from_quat(quaternion)
    matrix = r.as_matrix()
    return matrix

# 旋转矩阵转旋转向量
def matrix2rotvec(rotation_matrix):
    r = Rotation.from_matrix(rotation_matrix)
    rotvec = r.as_rotvec()
    return rotvec

# 旋转向量转旋转矩阵
def rotvec2matrix(rotvec):
    r = Rotation.from_rotvec(rotvec)
    matrix = r.as_matrix()
    return matrix

# 旋转向量转欧拉角
def rotvec2euler(rotvec):
    r = Rotation.from_rotvec(rotvec)
    euler = r.as_euler("xyz", True)
    return euler

def plot_transform_matrix(T, ax, scale, name):

    R = T[0:3, 0:3]
    # 绘制旋转后的坐标系
    new_origin = T[0:3, 3]
    new_x_axis = R[:, 0]
    new_y_axis = R[:, 1]
    new_z_axis = R[:, 2]

    ax.quiver(*new_origin, *new_x_axis, color='r', length=scale, normalize=True)
    ax.quiver(*new_origin, *new_y_axis, color='g', length=scale, normalize=True)
    ax.quiver(*new_origin, *new_z_axis, color='b', length=scale, normalize=True)

    # 设置图例
    ax.legend()
    ax.text(*new_origin, name)


if __name__=="__main__":

    R1 = np.array([[1, 0, 0],  
                   [0, 1, 0],  
                   [0, 0, 1]]) 

    a1 = np.deg2rad(90)
    a2 = np.deg2rad(90)
    a3 = np.deg2rad(0)

    # 绕x轴旋转a1度的旋转矩阵
    Rx = np.array([[1, 0, 0],  
                   [0, np.cos(a1), -np.sin(a1)],  
                   [0, np.sin(a1), np.cos(a1)]]) 

    # 绕Y轴旋转a2度的旋转矩阵
    Ry = np.array([[np.cos(a2), 0, np.sin(a2)],  
                   [0, 1, 0],  
                   [-np.sin(a2), 0, np.cos(a2)]])  
     
    # 绕Z轴旋转a3度的旋转矩阵  
    Rz = np.array([[np.cos(a3), -np.sin(a3), 0],  
                   [np.sin(a3), np.cos(a3), 0],  
                   [0, 0, 1]])   
    
    # 绕固定轴按XYZ顺序旋转
    R = Rz @ Ry @ Rx

    print(matrix2rotvec(R.T)) 
   

    '''cam1'''

    # cam1:T_cam_imu imu_to_cam1 (imu, cam1为单位矩阵)
    T_cam1_imu = np.array([
        [-0.006440212456178318, -0.9999663050460452, -0.005090425923179875, 0],
        [0.103752775587519, 0.004394862232823593, -0.9945934077520526, -0.01],
        [0.9945822666936133, -0.006933538671890227, 0.1037209757960279, -0.02],
        [0, 0, 0, 1]   
    ])

    # cam1:T_cam1_cam0 T_cn_cnm1 cam0_to_cam1 (cam0, cam1为单位矩阵)
    T_cam1_cam0 = np.array([
        [-0.003285363915904449, 0.9999792135042117, -0.0055478773819153115, -0.01],
        [-0.09509479515523178, -0.005835183050213633, -0.9954511191279869, -0.02],
        [-0.9954628000675521, -0.002742844923650234, 0.09511198916750835, -0.03],
        [0, 0, 0, 1]   
    ])

    T_cam1_cam0[0:3, 0:3] = R


    '''imu'''

    # T = [R, t], T的逆 = [R.T, -R.T*t]
    print(T_cam1_cam0.astype(np.float16))
    print(np.linalg.inv(T_cam1_cam0).astype(np.float16))
    print(-T_cam1_cam0[0:3,0:3].T @ T_cam1_cam0[0:3, 3])

    # imu:T_ic cam0_to_imu (cam0, imu为单位矩阵)
    # T_imu_cam0 = T_cam1_imu.inv @ T_cam1_cam0 （顺：从左到右，右乘）; T_cam02imu = T_imu2cam1.inv @ T_cam02cam1 (逆：从右到左，左乘)
    T_imu_cam0 = np.linalg.inv(T_cam1_imu).astype(np.float16) @ T_cam1_cam0

    # imu:T_ic cam1_to_imu (cam1, imu为单位矩阵)
    # T_imu_cam1 = T_cam1_imu.inv
    T_imu_cam1 = np.linalg.inv(T_cam1_imu).astype(np.float16)


    '''cam0'''

    # cam0:T_ci imu_to_cam0 (imu, cam0为单位矩阵)
    # T_cam0_imu = T_imu_cam0.inv
    T_cam0_imu = np.linalg.inv(T_imu_cam0).astype(np.float16)
    # T_cam0_imu = T_cam1_cam0.inv @ T_cam1_imu
    T_cam0_imu = np.linalg.inv(T_cam1_cam0).astype(np.float16) @ T_cam1_imu

    # cam0:T_cam0_cam1 T_cn_cnm1
    # T_cam0_cam1 = T_cam1_cam0.inv
    T_cam0_cam1 = np.linalg.inv(T_cam1_cam0).astype(np.float16)
    # T_cam0_cam1 = T_imu_cam0.inv @ T_imu_cam1
    T_cam0_cam1 = np.linalg.inv(T_imu_cam0).astype(np.float16) @ T_imu_cam1

    
    '''plot'''

    fig = plt.figure()
    ax = fig.add_subplot(projection='3d')

    # cam1
    plot_transform_matrix(np.identity(4), ax, 0.05, 'cam1')
    plot_transform_matrix(T_cam1_imu, ax, 0.05, 'imu')
    plot_transform_matrix(T_cam1_cam0, ax, 0.05, 'cam0')
    
    # imu
    plot_transform_matrix(np.identity(4), ax, 0.05, 'imu')
    plot_transform_matrix(T_imu_cam1, ax, 0.05, 'cam1')
    plot_transform_matrix(T_imu_cam0, ax, 0.05, 'cam0')

    # cam0
    plot_transform_matrix(np.identity(4), ax, 0.05, 'cam0')
    plot_transform_matrix(T_cam0_cam1, ax, 0.05, 'cam1')
    plot_transform_matrix(T_cam0_imu, ax, 0.05, 'imu')

    plt.axis('equal')
    plt.show()

发布Marker，在rviz上显示

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

import rospy
from visualization_msgs.msg import Marker

if __name__=="__main__":
    rospy.init_node("mark_node")
    rate = rospy.Rate(1)
    pub = rospy.Publisher("marker_py", Marker, queue_size=1)

    shape = Marker.CUBE

    while not rospy.is_shutdown():
        marker = Marker()

        marker.header.frame_id = "map"
        marker.header.stamp = rospy.Time.now()

        marker.ns = "basic_shapes"
        marker.id = 0

        marker.type = shape

        marker.action = Marker.ADD

        marker.pose.position.x = 0
        marker.pose.position.y = 0
        marker.pose.position.z = 0
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 0.0

        marker.scale.x = 1
        marker.scale.y = 1
        marker.scale.z = 1

        marker.color.r = 0.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.color.a = 1.0

        marker.lifetime = rospy.Duration()

        pub.publish(marker)

        if shape == Marker.CUBE:shape = Marker.SPHERE
        elif shape == Marker.SPHERE:shape = Marker.ARROW
        elif shape == Marker.ARROW:shape = Marker.CYLINDER
        elif shape == Marker.CYLINDER:shape = Marker.CUBE

        rate.sleep()

// basic_shapes.cpp

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

int main( int argc, char** argv )
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Rate r(1);
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("marker_cpp", 1);

  uint32_t shape = visualization_msgs::Marker::CUBE;

  while (ros::ok())
  {
    visualization_msgs::Marker marker;

    marker.header.frame_id = "map";
    marker.header.stamp = ros::Time::now();

    marker.ns = "basic_shapes";
    marker.id = 0;

    marker.type = shape;

    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    marker.lifetime = ros::Duration();

    while (marker_pub.getNumSubscribers() < 1)
    {
      if (!ros::ok())
      {
        return 0;
      }
      ROS_WARN_ONCE("Please create a subscriber to the marker");
      sleep(1);
    }
    marker_pub.publish(marker);

    switch (shape)
    {
    case visualization_msgs::Marker::CUBE:
      shape = visualization_msgs::Marker::SPHERE;
      break;
    case visualization_msgs::Marker::SPHERE:
      shape = visualization_msgs::Marker::ARROW;
      break;
    case visualization_msgs::Marker::ARROW:
      shape = visualization_msgs::Marker::CYLINDER;
      break;
    case visualization_msgs::Marker::CYLINDER:
      shape = visualization_msgs::Marker::CUBE;
      break;
    }

    r.sleep();
  }
}


/*
需在CMakeLists.txt中增加一些配置

find_package(catkin REQUIRED COMPONENTS
  roscpp
  visualization_msgs
)

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

*/

用MakerArray发布多个Marker

安装foxglove远程可视化工具（ROS1&&ROS2）

# 1.到官网下载安装包并安装
https://foxglove.dev/download
sudo apt install ./foxglove-studio-*.deb

# 2.rosbridge_server安装
# ROS2
sudo apt-get install ros-ROS2版本名称-rosbridge-suite
# ROS1
sudo apt-get install ros-ROS1版本名称-foxglove-bridge

# 3.将服务端与客户端置于同一网络下，远程调试使用网线连接提高通信效率。

# 4.启动WebSocket服务器，同时会输出监听的端口号（一般为port:9090）
# ROS2
ros2 launch rosbridge_server rosbridge_websocket_launch.xml
# ROS1
roslaunch foxglove_bridge foxglove_bridge.launch

# 5.打开Foxglove Studio，选择‘打开连接’，点击侧边栏‘Rosbridge’，更改对应WebSocket URL，包括两部分：服务端的本机ip地址+服务器监听的端口号。

关于安装Ubuntu时显示界面太小，有组件被遮挡的问题：按Alt+F7后移动鼠标可以把被遮挡的部分拖出来

关于NUC小电脑安装Ubuntu20.04后，无WIFI驱动的问题

# 注意：装完系统后不要用软件更新器更新Ubuntu核心组件，否则下列方法将无效

# 前期电脑没网可以暂时用数据线连接手机，通过USB网络共享给电脑提供网络

# 1.用软件和更新换源，建议用中科大ustc的

# 2.安装依赖
sudo apt install git make gcc

# 3.下载backport-iwlwifi并解压
https://git.kernel.org/pub/scm/linux/kernel/git/iwlwifi/backport-iwlwifi.git/snapshot/backport-iwlwifi-master.tar.gz

# 4.安装backport-iwlwifi
cd backport-iwlwifi-master
sudo make defconfig-iwlwifi-public
sudo make
sudo make install

# 5.下载linux-firmware
git clone https://mirrors.tuna.tsinghua.edu.cn/git/linux-firmware.git

# 6.复制到/lib/firmware/
cd linux-firmware
sudo cp iwlwifi-* /lib/firmware/

# 7.重启
reboot

# 以上步骤成功了也不能更新Ubuntu核心组件，不然WiFi又会消失

33.关于Ubuntu22.04接入ch340后，无/dev/ttyUSB*的问题，参考