How to get robot position by slam_out_pose?

asked 2020-08-19 03:47:24 -0600

Ridy gravatar image

hi please help, i made my own robot to evaluate position of the robot. i'm using Lidar hector_slam that release topic slam_out_pose. so i call the variable in position and orientation. but i'm not sure if i'm doing this correctly. i'm not using IMU or TICK i just want Lidar that can evaluate the position. if the robot pointing to the goal, the robot will go forward else rotate until the goal founded. the problem is when i try to rotate until exactly X position, it's not moving forward (print'forward')

robot specs;

  1. ydlidar x4
  2. Raspberry pi 3b+
  3. 2 DC Motor
  4. Motor Driver H Bridge


here's the screenshot of slam_out_pose;

#! /usr/bin/env python

import rospy
import RPi.GPIO as GPIO
import time
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Point
from math import atan2

x = 0.0
y = 0.0
theta = 0.0

def newOdom (msg):
    global x
    global y
    global theta

    x = msg.pose.position.x
    y = msg.pose.position.y

    rot_q = msg.pose.orientation
    (roll, pitch, theta) = euler_from_quaternion ([rot_q.x, rot_q.y, rot_q.z, rot_q.w])

sub = rospy.Subscriber("/slam_out_pose", PoseStamped, newOdom)


fright = GPIO.output(9,True)
bright = GPIO.output(10,True)
fright = GPIO.output(7,True)
bright = GPIO.output(8,True)

goal = Point ()
goal.x = 2
goal.y = 2

while not rospy.is_shutdown():
    inc_x = goal.x - x
    inc_y = goal.y - y

    angle_to_goal = atan2 (inc_y, inc_x)

    if abs(angle_to_goal - theta) > 0.1:
        print("ROTATE RIGHT")

i hope someone can explain to me thank you.

edit retag flag offensive close merge delete


Ridy gravatar image Ridy  ( 2020-08-19 03:48:46 -0600 )edit

I dont see you initialising your node: rospy.init_node("track_my_awesome_robot", log_level=rospy.INFO)

HappyBit gravatar image HappyBit  ( 2020-08-19 04:45:58 -0600 )edit

hi thank you for replying me, i initiate the code by directly "python" is this ok? so sorry i bit lost here

Ridy gravatar image Ridy  ( 2020-08-19 05:22:40 -0600 )edit

from Tutorials

rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this 
information, it cannot start communicating with the ROS Master.

Since you didnt initialise your node, I think you cant subscribe to a topic. This means you wont receive any messages.

The fact that you started the script using python doesnt change anything. It is however not the typical ROS way I prefer, which would be: rosrun <my_awesome_pkg>

HappyBit gravatar image HappyBit  ( 2020-08-19 05:49:25 -0600 )edit