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.