def __init__(self):
cnt = 0
tol = 0.1
tol_ang = 0.01
err_x = 0.0
err_y = 0.0
self.err_euc = 0.0
self.err_theta = 0.0
self.err_rot = 0.0
self.vel=Twist()
self.c_pos = Pose()
d_pos = Pose()
pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=10)
sub = rospy.Subscriber('turtle1/pose', Pose, self.PoseCallback)
rospy.init_node('sample_node')
loop_rate = rospy.Rate(10)
while not rospy.is_shutdown():
if cnt > 0:
d_pos_th = math.atan2(float(d_pos.y)-self.c_pos.y,float(d_pos.x)-self.c_pos.x)
self.err_theta = math.atan2(math.sin(d_pos_th - self.c_pos.theta),math.cos(d_pos_th - self.c_pos.theta));
err_x = float(d_pos.x) - self.c_pos.x
err_y = float(d_pos.y) - self.c_pos.y
self.err_euc = math.sqrt(math.pow(err_x,2) + math.pow(err_y,2))
ang = math.fmod(float(d_pos.theta),2*math.pi) - math.fmod(self.c_pos.theta, (2*math.pi))
if abs(ang) > math.pi:
self.err_rot = -np.sign(ang) * (math.pi*2 - abs(ang))
else:
self.err_rot = ang
cnt+=1
if abs(self.err_euc) < tol and abs(self.err_rot) < tol_ang:
disp_ang = math.fmod(self.c_pos.theta,2*math.pi)
if abs(disp_ang) > math.pi:
disp_ang = -np.sign(disp_ang) * (2*math.pi - abs(disp_ang))
print "---------------------------------------------"
print "Current pose [xc,yc,thc]=[%4.2f,%4.2f,%4.2f]" %(self.c_pos.x,self.c_pos.y,disp_ang)
print "Previous goal [xg,yg,thg]=[%4.2f,%4.2f,%4.2f]" %(float(d_pos.x),float(d_pos.y),math.fmod(float(d_pos.theta),2*math.pi))
print "Error, position:||xg-xc,yg-yc||=%4.2f, orientation:||thg-thc||=%4.2f" % (self.err_euc,self.err_rot)
print "---------------------------------------------\n"
d_pos.x, d_pos.y, d_pos.theta = raw_input("Enter desired goal pose (e.g., 10 10 2 means xg=10,yg=10,thg=2)\n").split()
elif abs(self.err_euc) >= tol:
self.move()
#print "%4.2f, %4.2f" %(self.vel.linear.x, self.vel.angular.z)
pub.publish(self.vel)
print "Turtle is moving!, distance to goal=%f" % (self.err_euc)
else:
self.rotate()
print "Turtle is rotating!, orientation error=%f" % (self.err_rot)
pub.publish(self.vel)
loop_rate.sleep()
def __init__(self):
#Creating our node,publisher and subscriber
rospy.init_node('turtlebot_controller',
anonymous=True) # creating a node
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist,
queue_size=10)
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose,
self.callback)
self.pose = Pose()
self.rate = rospy.Rate(10)
def __init__(self): # Konstruktor
# globale Klassen-Variablen instanzieren
self.pose = Pose()
self.goal = Pose()
self.vel_msg = Twist()
# Creates a node with name 'turtlebot_controller' and make sure it is a
# unique node (using anonymous=True).
rospy.init_node('turtlebot_controller', anonymous=True)
# Publisher which will publish to the topic '/turtle1/cmd_vel'.
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist,
queue_size=10)
# A subscriber to the topic '/turtle1/pose'. self.update_pose is called
# when a message of type Pose is received.
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose,
self.update_pose)
self.rate = rospy.Rate(10)
def __init__(self):
rospy.init_node('turtlebot_controller' , anonymous = True)
#initializing the turtle class, getting names from the launch file and making the corresponding publisher/subscriber
self.turtle = rospy.get_param('~turtle')
self.init_pose = Pose()
self.init_pose.x = rospy.get_param('~pose_x')
self.init_pose.y = rospy.get_param('~pose_y')
self.init_pose.theta = rospy.get_param('~pose_theta')
self.velocity_publisher = rospy.Publisher("/"+str(self.turtle)+"/cmd_vel", Twist, queue_size=1)
self.pose_subscriber = rospy.Subscriber("/"+str(self.turtle)+"/pose", Pose, self.update_pose)
self.pose = Pose()
#the spawn function will call the teleport function if the turtle does not exist
self.spawn()
def __init__(self):
rospy.init_node('turtlebot_controller', anonymous=True)
# Publisher which will publish to the topic '/turtle1/cmd_vel'.
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist, queue_size=10)
# A subscriber to the topic '/turtle1/pose'. self.update_pose is called
# when a message of type Pose is received.
self.pose_subscriber = rospy.Subscriber('/turtle1/pose',Pose, self.update_pose)
self.pose = Pose()
self.rate = rospy.Rate(10)
def __init__(self):
rospy.init_node('turtlebot_controller', anonymous=True)
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist, queue_size=10)
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, self.update_pose)
self.pose = Pose()
self.rate = rospy.Rate(10)
def move2goal(self):
"""Moves the turtle to the goal."""
goal_pose = Pose()
# Get the input from the user.
goal_pose.x = input("Set your x goal: ")
goal_pose.y = input("Set your y goal: ")
# Please, insert a number slightly greater than 0 (e.g. 0.01).
distance_tolerance = input("Set your tolerance: ")
vel_msg = Twist()
while self.euclidean_distance(goal_pose) >= distance_tolerance:
# Porportional controller.
# https://en.wikipedia.org/wiki/Proportional_control
# Linear velocity in the x-axis.
vel_msg.linear.x = self.linear_vel(goal_pose)
vel_msg.linear.y = 0
vel_msg.linear.z = 0
# Angular velocity in the z-axis.
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = self.angular_vel(goal_pose)
# Publishing our vel_msg
self.velocity_publisher.publish(vel_msg)
# Publish at the desired rate.
self.rate.sleep()
# Stopping our robot after the movement is over.
vel_msg.linear.x = 0
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
# If we press control + C, the node will stop.
rospy.spin()
def randomSpaw(self, data):
if (data.data == "i" or data.data == "I"):
random.seed()
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
self.pose2 = Pose()
if self.position == 0:
self.pose2.x = 9.0
self.pose2.y = 9.0
elif self.position == 1:
self.pose2.x = 2.5
self.pose2.y = 9.0
elif self.position == 2:
self.pose2.x = 2.5
self.pose2.y = 2.5
spawner(self.pose2.x, self.pose2.y, 0, 'turtle2')
self.state = True
self.timeInit = time.time()
self.pose1 = Pose()
self.pose1.x = -150
self.pose1.y = -150
elif (data.data == "r" or data.data == "R"):
if not self.finalState:
timeTotal = time.time() - self.timeInit
print("Tempo decorrido:", timeTotal)
f = open(self.folderPath + '/resultados.txt', 'w')
f.write('Status: Incompleto\r\nTempo: ' + str(timeTotal))
f.close()
screenShot = App()
screenShot.screenshot(self.folderPath, "imagem")
self.state = False
self.pub.publish("P")
self.finalState = False
self.position = self.position + 1
if self.position > 2:
self.position = 0
rospy.wait_for_service('reset')
reset = rospy.ServiceProxy('reset', Empty)
reset()
def __init__(self, target):
# Creates a node with name 'turtlebot_controller' and make sure it is a
# unique node (using anonymous=True).
self.goal_pose = Pose()
# Get the input from the user.
self.goal_pose.x = target[0]
self.goal_pose.y = target[1]
# Publisher which will publish to the topic '/turtle1/cmd_vel'.
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist,
queue_size=10)
# A subscriber to the topic '/turtle1/pose'. self.update_pose is called
# when a message of type Pose is received.
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose,
self.update_pose)
self.pose = Pose()
self.rate = rospy.Rate(10)
def __init__(self):
rospy.init_node('pub_marker_pose', anonymous=True)
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.get_marker)
self.pub = rospy.Publisher('/marker_pose', Pose, queue_size=10)
self.ar_pose = Pose()
self.target_id = -1
self.dist = 0
self.flag = 0
def __init__(self):
rospy.init_node("tb3control_node", anonymous=True)
self.vel_publisher = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
rospy.Subscriber("/odom", Odometry, self.update_pose)
rospy.Subscriber("/scan", LaserScan, self.update_scan)
self.pose = Pose()
self.rate = rospy.Rate(10)
self.max_vel = 0.22
self.max_ang = 2.84
def __init__(self):
self.poseflag = False
self.pose_subscriber = rospy.Subscriber("/tb3_0/amcl_pose",
PoseWithCovarianceStamped,
self.poseCallback,
queue_size=1)
self.turtlebot3_pose = Pose()
self.Positions_X = []
self.Positions_Y = []
#self.fig = plt.figure(figsize=(7,7), facecolor='w')
#self.fig.canvas.set_window_title('Trayectorias generadas')
plt.ion()
def __init__(self):
#Create node and make it unique
rospy.init_node('turtlebot_controller', anonymous=True)
#Publisher to turtle vel
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Pose, queue_size=10)
#Subscriber to pose - self update when Pose type message is received
self.pose_subscriber = rospy.Subscriber('/turtle/pose', Pose, self.update_pose)
self.pose = Pose()
self.rate = rospy.Rate(10)
def __init__(self):
#Creating our node,publisher and subscriber
rospy.init_node('turtlebot_controller', anonymous=True)
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist,
queue_size=10)
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose,
self.callback)
self.pose = Pose()
self.rate = rospy.Rate(10)
#This is the tolerance of the controller, because the turtlesim is not the most accurate simulator
self.tolerance = 0.1
def gridClean(publisher):
desired_pose = Pose()
desired_pose.x = 1
desired_pose.y = 1
desired_pose.theta = 0
go_to_goal(publisher, 1, 1)
setDesiredOrientation(publisher, 30, math.radians(desired_pose.theta))
for i in range(5):
move(publisher, 2.0, 1.0, True)
rotate(publisher, 20, 90, False)
move(publisher, 2.0, 9.0, True)
rotate(publisher, 20, 90, True)
move(publisher, 2.0, 1.0, True)
rotate(publisher, 20, 90, True)
move(publisher, 2.0, 9.0, True)
rotate(publisher, 20, 90, False)
pass
def __init__(self):
rospy.init_node('turtlebot', anonymous=False)
self.k_linear = 1.0
self.k_angular = 10.0
self.dist_thresh = 0.2
self.pose = Pose()
self.rate = rospy.Rate(10)
self.turtle_sub = rospy.Subscriber("/turtle1/pose", Pose, self.callback)
self.vel_pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=1)
def func_move_straight(self, param_dis, param_speed, param_dir):
obj_velocity_mssg = Twist()
obj_pose_mssg = Pose()
# Store the start position of the turtle
start_x = self._curr_x
start_y = self._curr_y
# Move the turtle till it reaches the desired position by publishing to Velocity topic
handle_pub_vel = rospy.Publisher(self._config_ros_pub_topic,
Twist,
queue_size=10)
# 1 Hz : Loop will its best to run 1 time in 1 second
var_loop_rate = rospy.Rate(10)
# Set the Speed of the Turtle according to the direction
if (param_dir == 'b'):
obj_velocity_mssg.linear.x = (-1) * abs(int(param_speed))
else:
obj_velocity_mssg.linear.x = abs(int(param_speed))
# Move till desired distance is covered
dis_moved = 0.0
while not rospy.is_shutdown():
# Send feedback to the client
obj_msg_feedback = myActionMsgFeedback()
obj_msg_feedback.cur_x = self._curr_x
obj_msg_feedback.cur_y = self._curr_y
obj_msg_feedback.cur_theta = self._curr_theta
self._sas.publish_feedback(obj_msg_feedback)
if ((dis_moved < param_dis)):
handle_pub_vel.publish(obj_velocity_mssg)
var_loop_rate.sleep()
dis_moved = abs(
math.sqrt(((self._curr_x - start_x)**2) +
((self._curr_y - start_y)**2)))
print('Distance Moved: {}'.format(dis_moved))
else:
break
# Stop the Turtle after desired distance is covered
obj_velocity_mssg.linear.x = 0
handle_pub_vel.publish(obj_velocity_mssg)
print('Destination Reached')
def CallBack(data):
n = len(data.transforms)
msg = Pose()
i = 0
while i < n:
k = data.transforms[i].fiducial_id
p = rospy.Publisher("/robot" + str(k) + "/edumip/pose",
Pose,
queue_size=3)
msg.x = data.transforms[i].transform.translation.x
msg.y = data.transforms[i].transform.translation.y
euler = euler_from_quaternion([
data.transforms[i].transform.rotation.x,
data.transforms[i].transform.rotation.y,
data.transforms[i].transform.rotation.z,
data.transforms[i].transform.rotation.w
])
msg.theta = euler[2]
p.publish(msg)
i = i + 1
def move2goal(self):
goal_pose = Pose()
goal_pose.x = input("x: ")
goal_pose.y = input("y: ")
disttolerance = input("disttolerance: ")
vel_msg = Twist()
while (self.euclidean_distance(goal_pose) >= disttolerance):
vel_msg.linear.x = self.linear_vel(goal_pose)
vel_msg.linear.y = 0.0
vel_msg.linear.z = 0.0
vel_msg.angular.x = 0.0
vel_msg.angular.y = 0.0
vel_msg.angular.z = self.angular_vel(goal_pose)
self.pub.publish(vel_msg)
self.rate.sleep(10)
vel_msg.linear.x = 0.0
vel_msg.angular.z = 0.0
self.pub.publish(vel_msg)
def __init__(self):
rospy.init_node(turtle_name)
self.goal_pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose,
self.update_goal_pose)
self.self_pose_subscriber = rospy.Subscriber('/chaser/pose', Pose,
self.update_self_pose)
self.velocity_publisher = rospy.Publisher('/chaser/cmd_vel',
Twist,
queue_size=10)
self.goal_pose = Pose()
self.distance_tolerance = 0.5
self.pose = Pose()
self.pose.x = random.random() * 10
self.pose.y = random.random() * 10
rospy.wait_for_service('/spawn')
spawn_func = rospy.ServiceProxy('/spawn', Spawn)
spawn_func(self.pose.x, self.pose.y, 0, turtle_name)
self.rate = rospy.Rate(100)
def __init__(self, name, i):
self.name = name
self.id = i
self.velocity_publisher = rospy.Publisher('/turtle{0}/cmd_vel'.format(
self.id),
Twist,
queue_size=10)
self.pose_subscriber = rospy.Subscriber(
'/turtle{0}/pose'.format(self.id), Pose, self.set_position)
self.pose = Pose()
def __init__(self):
rospy.init_node('move_to_marker', anonymous=True)
self.sb_odom = rospy.Subscriber('/odom', Odometry, self.get_odom)
self.sb_mark = rospy.Subscriber('/ar_pose_marker', AlvarMarkers,
self.get_marker)
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.ar_pose = Pose()
self.tb3pose = Pose()
self.rate = rospy.Rate(10)
self.prev_theta = 0.0
self.theta_sum = 0.0
self.tolerance = 0.15
self.Kp_dist = 1.0
self.Kp_ang = 1.0
self.target_found = False
def __init__(self):
rospy.init_node('pub_tb3_pose', anonymous=True)
self.sub = rospy.Subscriber('/odom', Odometry, self.get_odom)
self.pub = rospy.Publisher('/tb3pose', Pose, queue_size=10)
self.tb3pose = Pose()
self.rate = rospy.Rate(10)
self.prev_theta = 0.0
self.theta_sum = 0.0
def __init__(self):
# Crear un nodo con el nombre "turtlebot_controller"
# Asegurarse que el nombre sea unico (usando anonymoues = True)
rospy.init_node('turtlebot_controller', anonymous=True)
# La informacion sera publicada en el topic "/turtle1/cmd_vel
self.pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
# La informacion de posicion se tomara del topic "/turtle1/pose"
self.sub = rospy.Subscriber('/turtle1/pose', Pose, self.update_pose)
self.pose = Pose()
self.rate = rospy.Rate(10)
def triangle(self):
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist,
queue_size=20)
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose,
self.update_pose)
self.pose = Pose()
self.NumberOfSides = 3
self.distance = raw_input("Length of Shape?: ")
self.time = raw_input("Time to Finish?: ")
triangle.moveRight(self)
def move2goal(self): # Movimiento del robot
""" Mueve la tortuga al objetivo"""
goal_pose = Pose()
# Valores dados por el usuario
goal_pose.x = input("Coloca valor x objetivo: ")
goal_pose.y = input("Coloca valor y objetivo: ")
# Insetar un numero ligeramente mayor que 0 (e.g. 0.01).
distance_tolerance = input("Coloca valor de tolerancia: ")
vel_msg = Twist()
while self.euclidean_distance(goal_pose) >= distance_tolerance:
# Control proporcional
# Velocidad linear en el eje-x
vel_msg.linear.x = self.linear_vel(goal_pose)
vel_msg.linear.y = 0
vel_msg.linear.z = 0
# Velocidad angular en el eje-z
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = self.angular_vel(goal_pose)
# Publicar velocidad
self.pub.publish(vel_msg)
# Publicar la velocidad deseada
self.rate.sleep()
# Detener el robot cuando el movimiento ha terminado
vel_msg.linear.x = 0
vel_msg.angular.z = 0
self.pub.publish(vel_msg)
# Si presionamos Ctrl + c el movimiento se detendra
rospy.spin()
def move2goal(X, Y, tolerance):
"""Moves the turtle to the goal."""
pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, update_pose)
goal_pose = Pose()
pose = Pose()
goal_pose.x = X
goal_pose.y = Y
distance = math.sqrt(
math.pow((goal_pose.x - pose.x), 2) +
math.pow((goal_pose.y - pose.y), 2))
# Please, insert a number slightly greater than 0 (e.g. 0.01).
distance_tolerance = tolerance
vel_msg = Twist()
while distance >= distance_tolerance:
# Linear velocity in the x-axis.
vel_msg.linear.x = 1.5 * distance
vel_msg.linear.y = 0
vel_msg.linear.z = 0
# Angular velocity in the z-axis.
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 6 * (math.atan2(goal_pose.y - pose.y,
goal_pose.x - pose.x) - pose.theta)
# Publishing our vel_msg
velocity_publisher.publish(vel_msg)
# Stopping our robot after the movement is over.
vel_msg.linear.x = 0
vel_msg.angular.z = 0
velocity_publisher.publish(vel_msg)
# If we press control + C, the node will stop.
rospy.spin()
def marker_pose2d_cb(self, msg):
pose2d = Pose()
for msg in msg.markers:
if msg.id == TARGET_ID:
theta = self.get_marker_th(msg)
if (theta > 5.0):
pose2d.theta = theta - 2 * pi
elif (theta < -5.0):
pose2d.theta + 2 * pi
else:
pose2d.theta = theta
pose2d.x = msg.pose.pose.position.z * cos(
theta) # msg.pose.pose.position.z
pose2d.y = msg.pose.pose.position.z * sin(
theta) #-msg.pose.pose.position.x
self.marker_pose2d = pose2d
self.pub.publish(pose2d)
self.print_pose(pose2d)
"""
def trial_prepath(self, pose1, arr):
i = 0
cpose = Pose()
cpose.x = self.pose.x
cpose.y = self.pose.y
theta1 = self.steering_angle(pose1)
# print(arr)
while (1):
arr[i][0] = self.pose.x + (i + 1) * 0.05 * cos(theta1)
arr[i][1] = self.pose.y + (i + 1) * 0.05 * sin(theta1)
# print(arr[0][0]," ",arr[0][1])
# print(arr[i][0]," ",arr[i][1])
cpose.x = arr[i][0]
cpose.y = arr[i][1]
if (self.euclidean_distance(pose1, cpose) < 0.05):
index = i
break
i = i + 1
# print(arr)
y = 0
while (y < index):
print(arr[y][0], " ", arr[y][1], " ", y)
y = y + 1
return index
def spiralClean():
vel_msg = Twist()
count = 0
constant_speed = 4
vk = 1
wk = 2
rk = 0.5
loop_rate = rospy.Rate(1)
my_pose = Pose()
my_pose.x = 5.54
my_pose.y = 5.54
my_pose.theta = 0
moveGoal(my_pose, 0.01)
test = True
while (test):
rk += 0.5
vel_msg.linear.x = rk
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = constant_speed
velocity_publisher.publish(vel_msg)
loop_rate.sleep()
if ((x > 10) or (y > 10)):
test = False
vel_msg.linear.x = 0
velocity_publisher.publish(vel_msg)
def move2goal(self, xcoord, ycoord):
"""Moves the turtle to the goal."""
goal_pose = Pose()
# Set the goal to be the first node of the square
goal_pose.x = xcoord
goal_pose.y = ycoord
# Please, insert a number slightly greater than 0 (e.g. 0.01).
distance_tolerance = 0.1
vel_msg = Twist()
while self.euclidean_distance(goal_pose) > distance_tolerance:
# Proportional controller.
# https://en.wikipedia.org/wiki/Proportional_control
# Linear velocity in the x-axis.
vel_msg.linear.x = self.linear_vel(goal_pose)
vel_msg.linear.y = 0
vel_msg.linear.z = 0
# Angular velocity in the z-axis.
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = self.angular_vel(goal_pose)
# Publishing our vel_msg
self.velocity_publisher.publish(vel_msg)
# Publish at the desired rate.
#self.rate.sleep()
print("Done")
# Stopping our robot after the movement is over.
vel_msg.linear.x = 0
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
