ROS : Subscribe to /clicked_point in rviz for realtime waypoint

asked 2020-12-08 18:40:55 -0500

Varun gravatar image

updated 2020-12-10 08:01:31 -0500

Hello Everyone Hope you are doing well

I found out out that when you click publish point in rviz and do rostopic echo /clicked point we can get x,y coordinate of that point in rviz coordinate system

varun-flox@varunflox:~$ rostopic echo /clicked_point 
    header: 
      seq: 31
      stamp: 
        secs: 1607606021
        nsecs: 774185596
      frame_id: "map"
    point: 
      x: -4.99073648453
      y: -4.93864154816
      z: -0.00288152694702
    ---
    header: 
      seq: 32
      stamp: 
        secs: 1607606216
        nsecs: 838675541
      frame_id: "map"
    point: 
      x: -4.98981332779
      y: -4.96337604523
      z: -0.0113334655762
    ---

Subscriber node

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PointStamped
import tf
import random
import numpy as np

global x,y,z
x = 0.0
y = 0.0
z = 0.0

def callback(msg):

    point = PointStamped()
    point.header.stamp = rospy.Time.now()
    point.header.frame_id = "/map"
    point.point.x = msg.point.x      
    point.point.y = msg.point.y
    point.point.z = msg.point.z
    rospy.loginfo("coordinates:x=%f y=%f" %(point.point.x, point.point.y))

def listener():
    rospy.init_node('goal_publisher', anonymous=True)
    rospy.point_pub = rospy.Subscriber('/clicked_point', PointStamped, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

I already have a py code where we manually enter the way-points as a list to path_list representing x,y coordinates.

Is there any way i can get help to change that code to get real-time x,y coordinate from the /clicked_point topic please. Ideally not to manually set way-point in my code and use the /clicked_point to be directly subscribed in the code below and make the robot move to those rviz coordinates

Please refer: Similar question link

I have attached the main code

#! /usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point, Twist
import numpy as np

global x
global y
global rad_theta

x = 0.0
y = 0.0
rad_theta = 0.0

def newOdom(msg):
    global x, y, rad_theta

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

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

rospy.init_node ("speed_controller")

sub = rospy.Subscriber("/odometry/filtered", Odometry, newOdom)
pub = rospy.Publisher("/cmd_vel",Twist, queue_size=1)
speed = Twist()
r = rospy.Rate(10)

path_list = [(1,4), (4,4), (4, 1)]
point_index = 0  
goal = Point ()

while not rospy.is_shutdown():

    if point_index < len(path_list): # so we won't get an error of trying to reach non-existant index of a list
        goal.x = path_list[point_index][0] 
        goal.y = path_list[point_index][1] 
    else:
        point_index = 0 # I guess we're done?
        goal.x = path_list[point_index][0] 
        goal.y = path_list[point_index][1] 

    inc_x = goal.x - x
    inc_y = goal.y - y

    rad_target_bearing = np.arctan2(inc_y, inc_x) 
    target_bearing = np.rad2deg(rad_target_bearing) 
    my_heading = np.rad2deg(rad_theta)
    distance_to_goal_2 = np.sqrt(inc_x*inc_x + inc_y*inc_y) 

    if abs(target_bearing - my_heading) < 180:
        relative_bearing = target_bearing - my_heading 
    elif abs(target_bearing - my_heading) > 180 and my_heading > target_bearing:
        relative_bearing = (360 - my_heading) + target_bearing
    elif abs(target_bearing - my_heading) > 180 and my_heading < target_bearing:
        relative_bearing = 0 - my_heading - (360 - target_bearing ...
(more)
edit retag flag offensive close merge delete