多线激光雷达ndt定位与movebase结合的想法

最新推荐文章于 2024-11-03 11:09:36 发布

原创最新推荐文章于 2024-11-03 11:09:36 发布

· 594 阅读

7 ·

版权

文章标签：

#开发语言 #ros #python #自动驾驶 #机器人

ros 同时被 2 个专栏收录

64 篇文章

订阅专栏

python

26 篇文章

订阅专栏

请添加图片描述

movebase
针对movebase中tf变换关系的要求
里程计节点应发布 odom到 baselink的变换信息
定位节点，计算出 map到 baselink的变换关系，但不发布map到baselink的变换信息
定位节点应根据里程计节点发布的odom 到baselink的信息，计算出 map到 odom的变换信息并发布

如果定位节点计算 map到baselink的变换关系，并能配置发布一个 map到 baselink_footprint的信息，本节点可以根据定位节点和里程计节点，推算出 map到odom的关系，并发布tf

ndt定位
能结算出map到baselink的tf，修改部分代码不发布这个tf

static int _tf_use_baselink_footprint = 0;//zzz

  private_nh.getParam("tf_use_baselink_footprint", _tf_use_baselink_footprint);//zzz



    // Send TF "/base_link" to "/map"
    transform.setOrigin(tf::Vector3(current_pose.x, current_pose.y, current_pose.z));
    transform.setRotation(current_q);
    //    br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
    if (_use_local_transform == true)
    {
      if (_tf_use_baselink_footprint){//zzz add
        br.sendTransform(tf::StampedTransform(local_transform * transform, current_scan_time, "/map", "/base_link_footprint"));
      }else{
        br.sendTransform(tf::StampedTransform(local_transform * transform, current_scan_time, "/map", "/base_link"));
      }
    }
    else
    {
      if (_tf_use_baselink_footprint){//zzz add
        br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link_footprint"));
      }else{
        br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
      }
    }

适配节点

#!/usr/bin/env python
# -*- coding: utf-8 -*-


#import roslib

import rospy
import math
import tf
import geometry_msgs.msg
from geometry_msgs.msg import PoseStamped
import tf_conversions as tfc

#zzz------------------------------
#针对movebase中tf变换关系的要求
#里程计节点应发布 odom到 baselink的变换信息
#定位节点，计算出 map到 baselink的变换关系，但不发布map到baselink的变换信息
#定位节点应 根据 里程计节点发布的odom 到baselink的信息，计算出 map到 odom的变换信息并发布

#如果定位节点 计算 map到baselink的变换关系，并能配置发布一个 map到 baselink_footprint的信息，本节点可以根据定位节点和里程计节点，推算出 map到odom的关系，并发布tf


class MapOdom():

    def __init__(self):
        rospy.init_node('tf_map_odom', anonymous=True)
        self.listener = tf.TransformListener()
        self.ps_pub = rospy.Publisher("map_odom_pose", PoseStamped, queue_size=1)
        self.frame_map = rospy.get_param("~frame_map", "map")
        self.frame_odom = rospy.get_param("~frame_odom", "odom")
        self.frame_base_link = rospy.get_param("~frame_base_link", "base_link")
        self.frame_base_link_location = rospy.get_param("~frame_base_link_location", "base_link_footprint")

        self.test_location = rospy.get_param("~test_location", 1)
        self.rate_hz = rospy.get_param("~rate", 10)
        self.rate = rospy.Rate(self.rate_hz)
        x,y,z,w=0,0,0,1
        quaternion=(x,y,z,w)
        # 将四元数转换为旋转矩阵
        rotation_matrix = tf.transformations.quaternion_matrix(quaternion)
        print rotation_matrix

        rotation_matrix =tf.transformations.inverse_matrix(rotation_matrix)
        print rotation_matrix

        tx,ty,tz=1,2,3
        trans = [tx,ty,tz]
        translation_matrix = tf.transformations.translation_matrix(trans)

        rotation_matrix =tf.transformations.concatenate_matrices(translation_matrix, rotation_matrix)
        print rotation_matrix


    def tf_Q2M(self,x,y,z,w,tx,ty,tz):#输入四元数 + 位移
        
        # 将四元数转换为旋转矩阵
        rotation_matrix = tf.transformations.quaternion_matrix((x,y,z,w))
        translation_matrix = tf.transformations.translation_matrix((tx,ty,tz))
        return tf.transformations.concatenate_matrices(translation_matrix, rotation_matrix)
    #主要的计算：矩mo=逆（矩ob* 逆（矩mb））
    def run(self):
        #rospy.spin()

        c=1
        while(not rospy.is_shutdown()):
            self.rate.sleep()
            c=c+1
            print c
            if self.test_location:
                br = tf.TransformBroadcaster()
                br.sendTransform((0,0,0),
                         (0,0,0,1),
                         rospy.get_rostime(),
                         self.frame_base_link_location,
                         self.frame_map) 

            try:
                now = rospy.Time.now()


                #print self.listener.frameExists("base_link")
                #print self.listener.frameExists("odom")
                if self.listener.frameExists(self.frame_base_link) :#and self.listener.frameExists("map"):#20211229
                    # listener.waitForTransform("base_link", "map", rospy.Time(), rospy.Duration(4.0))
                    #t = self.listener.getLatestCommonTime("base_link", "odom")
                    #position, quaternion = self.listener.lookupTransform("odom","base_link",  t)
                    #获取odom---->baselink

                    t = self.listener.getLatestCommonTime(self.frame_base_link, self.frame_odom)
                    position, quaternion = self.listener.lookupTransform(self.frame_odom,self.frame_base_link,  t)
                    M_ob=self.tf_Q2M(quaternion[0],quaternion[1],quaternion[2],quaternion[3],position[0],position[1],position[2])



                    #t = self.listener.getLatestCommonTime("base_link_footprint", "map")
                    #position, quaternion = self.listener.lookupTransform("map","base_link_footprint",  t)
                    #获取map---->base_link_footprint（base_link）
                    t = self.listener.getLatestCommonTime(self.frame_base_link_location, self.frame_map)
                    position, quaternion = self.listener.lookupTransform(self.frame_map,self.frame_base_link_location,  t) 


                    M_mb=self.tf_Q2M(quaternion[0],quaternion[1],quaternion[2],quaternion[3],position[0],position[1],position[2])
                    M_bm=tf.transformations.inverse_matrix(M_mb)

                   
                    #map---->odom
                    
                    M_om=tf.transformations.concatenate_matrices(M_ob,M_bm)
                    M_mo=tf.transformations.inverse_matrix(M_om)
                    #print "zzzbbb"
                    #print M_mo
                    #print "zzzeee"
                    # 位移
                    T_mo=(M_mo[0][3],M_mo[1][3],M_mo[2][3])
                    # 将矩阵转换为四元数 
                    Q_mo=tf.transformations.quaternion_from_matrix(M_mo)
                    
                    #print Q_mo

                    # 发布tf 
                    br = tf.TransformBroadcaster()
                    br.sendTransform(T_mo,
                         Q_mo,
                         rospy.get_rostime(),
                         self.frame_odom,
                         self.frame_map) 

                else:
                    rospy.loginfo("frame not exists")
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                rospy.loginfo("failed: lookupTransform")
                continue





    #test
    def run2test(self):

        c=1
        while(not rospy.is_shutdown()):
            self.rate.sleep()
            c=c+1
            print c
            br = tf.TransformBroadcaster()
            br.sendTransform((0,0,0),
                         (0,0,0,1),
                         rospy.get_rostime(),
                         "base_link_footprint",
                         "map") 

            try:
                now = rospy.Time.now()


                print self.listener.frameExists("base_link")
                print self.listener.frameExists("odom")
                if self.listener.frameExists("base_link") :#and self.listener.frameExists("map"):#20211229
                    # listener.waitForTransform("base_link", "map", rospy.Time(), rospy.Duration(4.0))
                    t = self.listener.getLatestCommonTime("base_link", "odom")
                    position, quaternion = self.listener.lookupTransform("odom","base_link",  t)
                    #odom---->baselink


                    M_ob=self.tf_Q2M(quaternion[0],quaternion[1],quaternion[2],quaternion[3],position[0],position[1],position[2])

                    qInvert = tfc.transformations.quaternion_inverse
                    qMultiply = tfc.transformations.quaternion_multiply
                    inv_quat = qInvert(quaternion)
                    r_quat = qMultiply(inv_quat, quaternion)
                    # 对四元数进行逆变换
                    #inv_quat = tf.transformations.quaternion_inverse(quaternion)
                    #inv_quat = rospy.normalize_quaternion(inv_quat)
                    #inv_quat = rospy.quaternion_inverse(quaternion)
                    #r_quat=quaternion*inv_quat
                    pos =(position[0]*-1,position[1]*-1,position[2]*-1)#*-1
                    # rot=tf.transformations.quaternion_from_euler(self.pose_roll+self.adjust_roll,self.pose_pitch+self.adjust_pitch,self.pose_yaw+self.adjust_yaw)



                    br = tf.TransformBroadcaster()
                    br.sendTransform(pos,
                         r_quat,
                         rospy.get_rostime(),
                         "base_link_test",
                         "odom") 
                    print("逆变换后的四元数: x={}, y={}, z={}, w={}".format(inv_quat[0], inv_quat[1], inv_quat[2], inv_quat[3]))
                    e=quaternion*inv_quat
                    print("四元数: x={}, y={}, z={}, w={}".format(e[0], e[1], e[2], e[3]))
                    print position
                    print quaternion
                    pose = PoseStamped()
                    pose.header.stamp = rospy.Time.now()
                    pose.header.frame_id = "map"
                    pose.pose.position.x = position[0]
                    pose.pose.position.y = position[1]
                    pose.pose.position.z = position[2]

                    pose.pose.orientation.x = quaternion[0]
                    pose.pose.orientation.y = quaternion[1]
                    pose.pose.orientation.z = quaternion[2]
                    pose.pose.orientation.w = quaternion[3]
                    self.ps_pub.publish(pose)
                    rospy.loginfo("xyz| {},{},{}".format(position[0], position[1], position[2]))

                    t = self.listener.getLatestCommonTime("base_link_footprint", "map")
                    position, quaternion = self.listener.lookupTransform("map","base_link_footprint",  t)
                    M_mb=self.tf_Q2M(quaternion[0],quaternion[1],quaternion[2],quaternion[3],position[0],position[1],position[2])
                    M_bm=tf.transformations.inverse_matrix(M_mb)

                    inv_quat1 = qInvert(quaternion)
                    r_quat = qMultiply(inv_quat1, quaternion)

                    pos1 =(position[0]*-1,position[1]*-1,position[2]*-1)#*-1

                    
                    M_om=tf.transformations.concatenate_matrices(M_ob,M_bm)
                    M_mo=tf.transformations.inverse_matrix(M_om)
                    print "zzzbbb"
                    print M_mo
                    print "zzzeee"
                    T_mo=(M_mo[0][3],M_mo[1][3],M_mo[2][3])
                    Q_mo=tf.transformations.quaternion_from_matrix(M_mo)
                    
                    print Q_mo


                    br = tf.TransformBroadcaster()
                    br.sendTransform(T_mo,
                         Q_mo,
                         rospy.get_rostime(),
                         "odom",
                         "map") 

                    #inv_quat：Q2    inv_quat1：Q1
                    Q2=inv_quat1
                    Q1=inv_quat
                    #计算Q2相对于Q1的旋转。‌
                    #将Q1和Q2归一化，使其长度为1。这可以通过将四元数除以其模长来实现。

                    #计算Q2的共轭四元数Q2_conj。共轭四元数可以通过将Q2的虚部取负来得到。
                    Q2_conj=(Q2[0]*-1,Q2[1]*-1,Q2[2]*-1,Q2[3])
                    #计算相对旋转的四元数Q_relative = Q2_conj * Q1。这里的乘法是四元数的乘法运算。
                    Q_relative = qMultiply(Q2_conj, Q1)
                    #将Q_relative归一化，得到单位四元数。
                    #从单位四元数中提取旋转信息。可以通过将单位四元数转换为旋转矩阵或欧拉角来表示相对旋转。

                    #r_posi=(pos1[0]-pos[0],pos1[1]-pos[1],pos1[2]-pos[2])
                    r_posi=(pos[0]-pos1[0],pos[1]-pos1[1],pos[2]-pos1[2])

                    '''
                    br = tf.TransformBroadcaster()
                    br.sendTransform(r_posi,
                         Q_relative,
                         rospy.get_rostime(),
                         "odom",
                         "map") 
                    '''
                    #map---->baselink
                else:
                    rospy.loginfo("frame not exists")
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                rospy.loginfo("failed: lookupTransform")
                continue

        print "exit!!!!!!!!!!!"


if __name__ == '__main__':
    import os
    print os.environ['ROS_MASTER_URI']
    cp = MapOdom()
    cp.run()

3d点云转换成2d地图
roslaunch autolabor_navigation_launch 3d2dmap.launch

点云转换laser 要求points_noground话题
roslaunch pointcloud_to_laserscan pointcloud_to_laserscan.launch

laser里程计要求/lidar_16_scan话题，发布 odom->base_link
roslaunch laser_scan_matcher_odometry laser_scan_matcher_odometry.launch

多线雷达定位，配置tf_use_baselink_footprint不发布map->baselink 发布map->baselink_footprint
roslaunch lidar_localizer ndt_matching.launch method_type:=0 use_odom:=False use_imu:=False imu_upside_down:=False imu_topic:= get_height:=False output_log_data:=False tf_use_baselink_footprint:=1

计算 map->odom的变换, 依赖tf：定位map->baselink_footprint和里程计 odom->base_link
roslaunch r2ware_car_labor r2ware_tf_map_odom.launch

多线雷达movebase导航
roslaunch autolabor_navigation_launch r2ware_navigation_lidar_16.launch

仅需线控底盘和多线激光雷达，movebase导航走起～