Localization odometry movement in the Z axis

[Prince Pereira]

Is there a way to ensure that the frame for odometry e.g. velodyne frame is always fixed in a certain part of the Z axis, because even though my map that i made is relatively flat with ground being 0 on z axis, im in a narrow indoor compound and this results in the localization outputting odometry that causes my base_footprint to go below ground level or above ground level which then affects the ability of my sensor which is tilted at an angle to the ground to detect obstacles as it is not supposed to detect the ground as an obstacle

If there is no way to force this, what are ways to minimize this from happening?

[Prince Pereira]

I made a simple odom publisher to negate any Z-axis movement and any roll and pitch movement following the velodyne frame which is assumed that map ---> velodyne transform is the first transform for the localization this makes a new frame base_link_intermediate which will be fixed at z=0, which can be used as reference frames for all other sensors. This is useful if implementing move_base with 2dcostmap as 2dcostmap have fixed height reference frames for obstacle detection

[Prince Pereira]

`
import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3

class odom_intermediate(object):
def init(self):
self.fixed_height = 0
self.posx = 0
self.posy = 0
self.posz = 0
self.quaternion_out = (0,0,0,1)
self.sub = rospy.Subscriber("/odom", Odometry, self.sub_callback)
self.pub = rospy.Publisher("/odom_intermediate", Odometry, queue_size=10)
self.odom_intermediate = Odometry()
self.odom_broadcaster = tf.TransformBroadcaster()
self.pub_data()

def sub_callback(self, msg):
    self.posx = 0
    self.posy = 0
    # cancel out motions in the z axis so that velodyne intermediate is always 1.8
    self.posz = self.fixed_height - msg.pose.pose.position.z
    # calculating the quaternion to rpy to cancel out r and p motions)
    quaternion_in = (
        msg.pose.pose.orientation.x,
        msg.pose.pose.orientation.y,
        msg.pose.pose.orientation.z,
        msg.pose.pose.orientation.w)
    
    euler_in = tf.transformations.euler_from_quaternion(quaternion_in)
    self.quaternion_out = tf.transformations.quaternion_from_euler(-euler_in[0],-euler_in[1], 0)
    
def pub_data(self):
    while not rospy.is_shutdown():
        #we will publish the transform over tf
        self.odom_broadcaster.sendTransform(
            (self.posx, self.posy, self.posz),
            self.quaternion_out,
            rospy.Time.now(),
            "base_link_intermediate",
            "velodyne"
        )
        # start publishing odometry message over ROS
        self.odom_intermediate.header.stamp = rospy.Time.now()
        self.odom_intermediate.header.frame_id = "velodyne"

        #set the position in odometry
        self.odom_intermediate.pose.pose = Pose(Point(self.posx, self.posy, self.posz), Quaternion(*self.quaternion_out))

        #set child frame
        self.odom_intermediate.child_frame_id = "base_link_intermediate"

        self.pub.publish(self.odom_intermediate)

if name == "main":
rospy.init_node("odom_intermediate", anonymous=True)
odom_intermediate_object = odom_intermediate()
rospy.spin()

`