Last active
May 29, 2023 09:10
-
-
Save robodhruv/33c23f0299bbac359c6975c23923bd3e to your computer and use it in GitHub Desktop.
ROS Node for converting nav_msgs/odometry messages to nav_msgs/Path
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
from __future__ import print_function | |
import rospy | |
from tf.transformations import quaternion_from_euler | |
from std_msgs.msg import String | |
from nav_msgs.msg import Odometry, Path | |
from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped | |
from sensor_msgs.msg import Joy | |
import sys | |
import json | |
from collections import deque | |
import time | |
def callback(data): | |
global xAnt | |
global yAnt | |
global cont | |
pose = PoseStamped() | |
pose.header.frame_id = "main" | |
pose.pose.position.x = float(data.pose.pose.position.x) | |
pose.pose.position.y = float(data.pose.pose.position.y) | |
pose.pose.position.z = float(data.pose.pose.position.z) | |
pose.pose.orientation.x = float(data.pose.pose.orientation.x) | |
pose.pose.orientation.y = float(data.pose.pose.orientation.y) | |
pose.pose.orientation.z = float(data.pose.pose.orientation.z) | |
pose.pose.orientation.w = float(data.pose.pose.orientation.w) | |
if (xAnt != pose.pose.position.x and yAnt != pose.pose.position.y): | |
pose.header.seq = path.header.seq + 1 | |
path.header.frame_id = "main" | |
path.header.stamp = rospy.Time.now() | |
pose.header.stamp = path.header.stamp | |
path.poses.append(pose) | |
# Published the msg | |
cont = cont + 1 | |
rospy.loginfo("Hit: %i" % cont) | |
if cont > max_append: | |
path.poses.pop(0) | |
pub.publish(path) | |
xAnt = pose.pose.orientation.x | |
yAnt = pose.pose.position.y | |
return path | |
if __name__ == '__main__': | |
# Initializing global variables | |
global xAnt | |
global yAnt | |
global cont | |
xAnt = 0.0 | |
yAnt = 0.0 | |
cont = 0 | |
# Initializing node | |
rospy.init_node('path_plotter') | |
# Rosparams set in the launch (can ignore if running directly from bag) | |
# max size of array pose msg from the path | |
if not rospy.has_param("~max_list_append"): | |
rospy.logwarn('The parameter max_list_append dont exists') | |
max_append = rospy.set_param("~max_list_append", 1000) | |
max_append = 1000 | |
if not (max_append > 0): | |
rospy.logwarn('The parameter max_list_append is not correct') | |
sys.exit() | |
pub = rospy.Publisher('/path', Path, queue_size=1) | |
path = Path() | |
msg = Odometry() | |
# Subscription to the required odom topic (edit accordingly) | |
msg = rospy.Subscriber('/eskf_odom', Odometry, callback) | |
rate = rospy.Rate(30) # 30hz | |
try: | |
while not rospy.is_shutdown(): | |
# rospy.spin() | |
rate.sleep() | |
except rospy.ROSInterruptException: | |
pass |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
is this working?
xAnt
is orientation or position?and why and instead of or if only for the start position?