ROS : Subscribe to /clicked_point in rviz for realtime waypoint
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 ...