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导航走起～
3349
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
