Ask Your Question
0

what do i need to do to publish odom into rviz?

asked 2020-09-03 13:17:11 -0500

MisticFury gravatar image

updated 2020-09-03 13:17:58 -0500

im using a pixhawk in my project and the pixhawk comes with a gyro, acceleratormeter , magnetometer, barometer, and gps. im using ardupilot on the board so it has ekf on it. but no matter where i look i cant figure out how i need to publish the topic for rviz or how to feed it the raw data. im using pymavlink and dronkit for this in python3.

edit retag flag offensive close merge delete

Comments

1

Sorry, I'm confused. Do you want to know what it is necessary to build an odometry message, or you already have it and now you want to plot it in RVIZ?

mirella melo gravatar image mirella melo  ( 2020-09-03 23:28:57 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-09-04 01:55:17 -0500

Nitesh_j gravatar image

I would suggest u go through this link textfor more info , after understanding the basics of it, try the below python script

#!/usr/bin/env python

import math
from math import sin, cos, pi

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

rospy.init_node('odometry_publisher')

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()

x = 0.0
y = 0.0
th = 0.0

vx = 0.1
vy = -0.1
vth = 0.1

current_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(1.0)
while not rospy.is_shutdown():
    current_time = rospy.Time.now()

    # compute odometry in a typical way given the velocities of the robot
    dt = (current_time - last_time).to_sec()
    delta_x = (vx * cos(th) - vy * sin(th)) * dt
    delta_y = (vx * sin(th) + vy * cos(th)) * dt
    delta_th = vth * dt

    x += delta_x
    y += delta_y
    th += delta_th

    # since all odometry is 6DOF we'll need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

    # first, we'll publish the transform over tf
    odom_broadcaster.sendTransform(
        (x, y, 0.),
        odom_quat,
        current_time,
        "base_link",
        "odom"
    )

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_link"
    odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))

    # publish the message
    odom_pub.publish(odom)

    last_time = current_time
    r.sleep()
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-09-03 13:17:11 -0500

Seen: 477 times

Last updated: Sep 04 '20