Hi, I am trying to use the code of the example call “navigation.py” to move the robot to a goal, but does not work. I copied in the folder that I create with the name go_to, in the folder stretch_navigation, but when i try to run with rosrun the robot do not move and after run the code do not work send a goal in Rviz with the 2D Nav Goal ¿could you help with this problem?
Hi @Humberto_Andres_Hida, happy to help here. Could you link to the example code you are using, or include it here? Furthermore, please include what output you see when running the “rosrun” command.
1 Like
i already fix the problem with the original code that you have to move to a goal, the problem was the need of some dependencies. Now I am doing a code to search in different goals base on the probability that the object, which the robot is looking for is near to that point/goal, the code is the next:
#!/usr/bin/env python3
import rospy
import actionlib
import sys
import pickle
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Quaternion,Pose
from tf import transformations
import math
import time
import transforms3d.quaternions as quaternions
import transforms3d.quaternions as euler
class Points:
def __init__(self,id,x,y,weight,C1,C2,C3,C4,C5):
self.id = id
self.x = x
self.y = y
self.weight = weight
self.C1 = C1
self.C2 = C2
self.C3 = C3
self.C4 = C4
self.C5 = C5
"""class list:
def points_order(self,i):
root='src/stretch_ros/maps/points/reference_points_3.pickle'
#root='reference_points_3.pickle'
with open(root, 'rb') as file:
list = pickle.load(file)
high_weight=sorted(list, key=lambda list: list.weight,reverse=True)
print(high_weight)
print("ID:",high_weight[0].id)
print("X:",high_weight[0].weight)
print("puntos:",len(high_weight))
return [high_weight[i].x,high_weight[i].y]"""
class StretchNavigation:
"""
A simple encapsulation of the navigation stack for a Stretch robot.
"""
def __init__(self):
"""
Create an instance of the simple navigation interface.
:param self: The self reference.
"""
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
rospy.loginfo('{0}: Made contact with move_base server'.format(self.__class__.__name__))
self.goal = MoveBaseGoal()
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time()
self.goal.target_pose.pose.position.x = 0.0
self.goal.target_pose.pose.position.y = 0.0
self.goal.target_pose.pose.position.z = 0.0
self.goal.target_pose.pose.orientation.x = 0.0
self.goal.target_pose.pose.orientation.y = 0.0
self.goal.target_pose.pose.orientation.z = 0.0
self.goal.target_pose.pose.orientation.w = 1.0
self.position=None
self.orientation=None
def get_quaternion(self,theta):
"""
A function to build Quaternians from Euler angles. Since the Stretch only
rotates around z, we can zero out the other angles.
:param theta: The angle (radians) the robot makes with the x-axis.
"""
return Quaternion(*transformations.quaternion_from_euler(0.0, 0.0, theta))
def get_euler_angle(self,x,y,z,w):
rot_matrix = quaternions.quat2mat([w,x,y,z])
return euler.mat2quat(rot_matrix)
def distance(self,x1,x2,y1,y2):
return math.sqrt((x1-x2)**2+(y1-y2)**2)
def go_to(self):
"""
Drive the robot to a particular pose on the map. The Stretch only needs
(x, y) coordinates and a heading.
:param x: x coordinate in the map frame.
:param y: y coordinate in the map frame.
:param theta: heading (angle with the x-axis in the map frame)
"""
rospy.Subscriber('robot_pose', Pose, nav.position_callback)
rospy.Subscriber('robot_pose', Pose, nav.orientation_callback)
root='src/stretch_ros/maps/points/reference_points_3.pickle'
#root='reference_points_3.pickle'
with open(root, 'rb') as file:
list = pickle.load(file)
high_weight=sorted(list, key=lambda list: list.weight,reverse=True)
"""print(high_weight)
print("ID:",high_weight[0].id)
print("X:",high_weight[0].weight)
print("puntos:",len(high_weight))"""
"""nav.listener(nav.position_callback)
position=nav.listener(nav.position_callback)
nav.listener(nav.orientation_callback)
orientation=nav.listener(nav.orientation_callback)"""""
for s in range(len(high_weight)):
# Update theta
region_probability = 0
theta = 0
for i in range (5):
if high_weight[s].C1 < region_probability:
region_probability=high_weight[s].C1
theta = math.pi/2
if high_weight[s].C2 < region_probability:
region_probability=high_weight[s].C2
theta = math.pi/2+(2/5)*math.pi
if high_weight[s].C3 < region_probability:
region_probability=high_weight[s].C3
theta = math.pi/2+(4/5)*math.pi
if high_weight[s].C4 < region_probability:
region_probability=high_weight[s].C4
theta = math.pi/2+(6/5)*math.pi
if high_weight[s].C5 < region_probability:
region_probability=high_weight[s].C5
theta = math.pi/2+(8/5)*math.pi
# Go to point
#rospy.loginfo('{0}: Heading for ({1}, {2}) at {3} radians'.format(self.__class__.__name__,x, y, theta))
self.goal.target_pose.pose.position.x = high_weight[s].x
self.goal.target_pose.pose.position.y = high_weight[s].y
self.goal.target_pose.pose.orientation = self.get_quaternion(theta)
print(self.goal)
self.client.send_goal(self.goal, done_cb=self.done_callback)
self.client.wait_for_result()
# While until reached
point_reached = False
while not point_reached:
distance_treshoald= 0.01
theta_treshoald=(1/36)*math.pi
orientation=self.orientation
position=self.position
#distance=math.sqrt((high_weight[s].x-position[0])**2+(high_weight[s].y-position[1])**2)
diference_of_angle=theta-self.get_euler_angle(orientation[3],orientation[0],orientation[1],orientation[2])
# Check if reached
if distance_treshoald<self.distance(high_weight[s].x,position[0],high_weight[s].y,position[1]) and diference_of_angle<theta_treshoald:
print("Point reached: ","ID: ",high_weight[s].id,"Weight: ",high_weight[s].weight)
point_reached = True
# Sleep
time.sleep(1)
def done_callback(self, status, result):
"""
The done_callback function will be called when the joint action is complete.
:param self: The self reference.
:param status: status attribute from MoveBaseActionResult message.
:param result: result attribute from MoveBaseActionResult message.
"""
if status == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo('{0}: SUCCEEDED in reaching the goal.'.format(self.__class__.__name__))
else:
rospy.loginfo('{0}: FAILED in reaching the goal.'.format(self.__class__.__name__))
def position_callback(self,pose):
self.position=pose.position
def orientation_callback(self,pose):
self.orientation=pose.orientation
def listener(self,callback):
rospy.init_node('listener',anonymous=True)
rospy.Subscriber('robot_pose', Pose, callback)
"""def callback(pose):
position=pose.position
orientation=pose.orientation"""
if __name__ == '__main__':
rospy.init_node('navigation', argv=sys.argv)
nav = StretchNavigation()
nav.go_to()
But i have problem because it says that some of the variables that I use to check that the robot is close enough to send other goal are not subscribable.
Hi @Humberto_Andres_Hida, you have the following subscribers on “robot_pose”:
rospy.Subscriber('robot_pose', Pose, nav.position_callback)
rospy.Subscriber('robot_pose', Pose, nav.orientation_callback)
Do you have a topic called ‘robot_pose’? If you’re using Stretch Navigation, I believe the topic is called “amcl_pose”. AMCL is the particle filter that tracks the robot’s location in Stretch Navigation. Here’s a link to the AMCL docs:
1 Like
i didnt use the AMCL, i used the next code:
#!/usr/bin/env python3
# roslaunch the stretch launch file beforehand
import rospy
import tf
import geometry_msgs
from geometry_msgs.msg import Pose
if __name__ == '__main__':
rospy.init_node('transformations')
listener = tf.TransformListener()
from_frame_rel ='base_link'
to_frame_rel='map'
#from_frame_rel = 'map'
#to_frame_rel = 'base_link'
rospy.sleep(1.0)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
try:
translation, rotation = listener.lookupTransform(to_frame_rel, from_frame_rel, rospy.Time(0))
#rospy.loginfo('The pose of target frame %s with respect to %s is: \n %s, %s', to_frame_rel, from_frame_rel, translation, rotation)
pub = rospy.Publisher('robot_pose', Pose, queue_size=1)
rate = rospy.Rate(2) # Hz
pose=Pose()
pose.position.x=translation[0]
pose.position.y=translation[1]
pose.position.z=translation[2]
pose.orientation.x=rotation[0]
pose.orientation.y=rotation[1]
pose.orientation.z=rotation[2]
pose.orientation.w=rotation[3]
pub.publish(pose)
#print("Translation:",translation,"Rotation:",rotation)
print("x: ",translation[0],"y: ",translation[1],"orientation z",rotation[2])
rate.sleep()
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
I will try then with the AMCL
Thanks I try with the amcl_pose and its works better for what i want to do
1 Like