def compare_axis_angle(angle1, axis1, angle2, axis2, decimal=3):
try:
np.testing.assert_array_almost_equal(axis1, axis2, decimal=decimal)
np.testing.assert_almost_equal(shortest_angular_distance(
angle1, angle2),
0,
decimal=decimal)
except AssertionError:
try:
np.testing.assert_array_almost_equal(axis1,
-axis2,
decimal=decimal)
np.testing.assert_almost_equal(shortest_angular_distance(
angle1, abs(angle2 - 2 * pi)),
0,
decimal=decimal)
except AssertionError:
np.testing.assert_almost_equal(shortest_angular_distance(
angle1, 0),
0,
decimal=decimal)
np.testing.assert_almost_equal(shortest_angular_distance(
0, angle2),
0,
decimal=decimal)
assert not np.any(np.isnan(axis1))
assert not np.any(np.isnan(axis2))
def _assign_constant_velocity_profile(self, traj, max_joint_vel):
""" Assigns a constant velocity profile to a moveit_msgs/RobotTrajectory """
t = 0.0
for i in range(1, len(traj.joint_trajectory.points)):
p_prev = traj.joint_trajectory.points[i - 1]
p = traj.joint_trajectory.points[i]
num_dof = len(p_prev.positions)
max_joint_dist = 0.0
for j in range(num_dof):
dist = math.fabs(
angles.shortest_angular_distance(p_prev.positions[j],
p.positions[j]))
max_joint_dist = max(max_joint_dist, dist)
dt = max_joint_dist / max_joint_vel
p.velocities = num_dof * [0.0]
for j in range(num_dof):
dist = math.fabs(
angles.shortest_angular_distance(p_prev.positions[j],
p.positions[j]))
p.velocities[j] = dist / dt
t += dt
p.time_from_start = rospy.Duration(t)
def compare_axis_angle(actual_angle,
actual_axis,
expected_angle,
expected_axis,
decimal=3):
try:
np.testing.assert_array_almost_equal(actual_axis,
expected_axis,
decimal=decimal)
np.testing.assert_almost_equal(shortest_angular_distance(
actual_angle, expected_angle),
0,
decimal=decimal)
except AssertionError:
try:
np.testing.assert_array_almost_equal(actual_axis,
-expected_axis,
decimal=decimal)
np.testing.assert_almost_equal(shortest_angular_distance(
actual_angle, abs(expected_angle - 2 * pi)),
0,
decimal=decimal)
except AssertionError:
np.testing.assert_almost_equal(shortest_angular_distance(
actual_angle, 0),
0,
decimal=decimal)
np.testing.assert_almost_equal(shortest_angular_distance(
0, expected_angle),
0,
decimal=decimal)
assert not np.any(np.isnan(actual_axis))
assert not np.any(np.isnan(expected_axis))
def test_rotation_distance(self, q1, q2):
m1 = quaternion_matrix(q1)
m2 = quaternion_matrix(q2)
actual_angle = w.compile_and_execute(w.rotation_distance, [m1, m2])
expected_angle, _, _ = rotation_from_matrix(m1.T.dot(m2))
expected_angle = expected_angle
try:
self.assertAlmostEqual(shortest_angular_distance(actual_angle, expected_angle), 0, places=3)
except AssertionError:
self.assertAlmostEqual(shortest_angular_distance(actual_angle, -expected_angle), 0, places=3)
def cartesian_control(robot_pose, goal, K_v, K_omega):
"""
This function computes the control signal to guides the
robot to the desired goal. It's based on the Cartesian
Control Algorithm
"""
# Computing the position error
error_x = goal.x - robot_pose.x
error_y = goal.y - robot_pose.y
error_lin = round(math.sqrt(error_x**2 + error_y**2), 3)
v = K_v * error_lin
# Computing the heading
heading = math.atan2(error_y, error_x)
error_th = round(
angles.shortest_angular_distance(robot_pose.theta, heading), 3)
omega = K_omega * error_th
print('Error lin:', error_lin, 'Errol heading:', error_th)
u = Twist()
u.linear.x = v
u.angular.z = omega
return u
def robot_command(robot_odom, goal, gain):
# recuperando as coordenadas do robo
x = robot_odom.x
y = robot_odom.y
theta = robot_odom.theta
# recuperand o goal
x_d = goal.x
y_d = goal.y
theta_d = goal.theta
# recuperando os ganhos
K_v = gain[0]
K_omega = gain[1]
# definindo os erros
delta_x = x_d - x
delta_y = y_d - y
erro_p = round(math.sqrt(delta_x**2 + delta_y**2), 3)
heading = round(math.atan2(delta_y, delta_x), 3)
erro_theta = angles.shortest_angular_distance(theta, heading)
v = K_v * erro_p
omega = K_omega * erro_theta
robot_vel = Twist()
robot_vel.linear.x = v
robot_vel.angular.z = omega
return robot_vel
def drive_to(self, place, claw_offset=0, **kwargs):
'''Drive directly to a particular point in space. The point must be in
the odometry reference frame.
Arguments:
* `place`: (`geometry_msgs.msg.Point` or `geometry_msgs.msg.Pose2D`): The place to drive.
Keyword Arguments/Returns/Raises:
* See `mobility.swarmie.Swarmie.drive`
* claw_offset to the odometry reference frame. Appropriate value
to be passed in, otherwise the reference frame remains unchanged.
'''
loc = self.get_odom_location().get_pose()
dist = math.hypot(loc.y - place.y, loc.x - place.x)
angle = angles.shortest_angular_distance(
loc.theta, math.atan2(place.y - loc.y, place.x - loc.x))
req = MoveRequest(
theta=angle,
r=dist - claw_offset,
)
return self.__drive(req, **kwargs)
def test_shorted_angular_distance(self, f1, f2):
angle1 = np.pi * f1
angle2 = np.pi * f2
ref_distance = shortest_angular_distance(angle1, angle2)
distance = speed_up_and_execute(spw.shortest_angular_distance, [angle1, angle2])
self.assertAlmostEqual(distance, ref_distance, places=7)
assert abs(distance) <= np.pi
def recover():
"""Recover from difficult situations:
- No home tags are in view anymore
- The tag to drive to is too close (we might be inside of home)
Raises:
InsideHomeException: if we're stuck inside of home.
"""
# TODO: should sonar be ignored when recovering?
# TODO: is simply backing up a little bit a reliable recovery move?
ignore = (Obstacle.TAG_TARGET | Obstacle.TAG_HOME |
Obstacle.INSIDE_HOME | Obstacle.IS_SONAR)
check_inside_home()
if swarmie.has_home_odom_location():
home_odom = swarmie.get_home_odom_location()
loc = swarmie.get_odom_location().get_pose()
angle = angles.shortest_angular_distance(loc.theta,
math.atan2(home_odom.y - loc.y,
home_odom.x - loc.x))
# If the rover is facing away from home's center, turn toward it
if abs(angle) > math.pi / 3:
swarmie.turn(angle, ignore=ignore)
return
swarmie.drive(-.1, ignore=ignore)
def test_shorted_angular_distance(self, angle1, angle2):
try:
expected = shortest_angular_distance(angle1, angle2)
except ValueError:
expected = np.nan
actual = w.compile_and_execute(w.shortest_angular_distance, [angle1, angle2])
self.assertTrue(np.isclose(actual, expected, equal_nan=True))
def polar_ctrl2(position, destination, K_polar):
# Polar coordinate control function
# remaping input variables
delta_x = destination.point.x
delta_y = destination.point.y
theta = position.theta
delta_theta = round(destination.point.z, 3)
k_rho = K_polar[0]
k_alpha = K_polar[1]
k_beta = K_polar[2]
rho = math.sqrt(math.pow(delta_x, 2) + math.pow(delta_y, 2))
alpha = math.atan2(delta_y, delta_x) + math.pi
beta = alpha + theta
beta = angles.shortest_angular_distance(alpha, theta)
print 'rho: ', rho
print 'alpha: ', alpha
print 'beta: ', beta, '----\n'
v = k_rho * rho * math.cos(alpha)
w = (k_alpha * alpha) + (k_rho *
((math.sin(alpha) * math.cos(alpha)) / alpha) *
(alpha + (k_beta * beta)))
U_uni = [v, w]
return U_uni
def polar_ctrl(position, destination, K_polar):
# Polar coordinate control function
# remaping input variables
x = destination.point.x
y = destination.point.y
theta = position.theta
theta_goal = round(destination.point.z, 3)
# Remaping the gain variables
k_rho = K_polar[0]
k_alpha = K_polar[1]
k_beta = K_polar[2]
# Computing the coordinate transformation
rho = math.sqrt(x**2 + y**2)
alpha = math.atan2(y, x)
beta = round(angles.shortest_angular_distance(theta_goal, theta), 3)
# Defining the position threshold
if abs(rho) < 0.15:
alpha = 0
rho = 0
# Computin the control law
v = (k_rho * rho)
w = ((k_alpha * alpha) + (k_beta * beta))
agl = [rho, alpha, beta] # Secondarie monitiring parameters
U_uni = [v, w, agl]
return U_uni
def drive_to(self, place, claw_offset=0, **kwargs):
'''Drive directly to a particular point in space. The point must be in
the odometry reference frame.
Arguments:
* `place`: (`geometry_msgs.msg.Point` or `geometry_msgs.msg.Pose2D`): The place to drive.
Keyword Arguments/Returns/Raises:
* See `mobility.swarmie.Swarmie.drive`
* claw_offset to the odometry reference frame. Appropriate value
to be passed in, otherwise the reference frame remains unchanged.
'''
loc = self.get_odom_location().get_pose()
dist = math.hypot(loc.y - place.y, loc.x - place.x)
angle = angles.shortest_angular_distance(
loc.theta, math.atan2(place.y - loc.y, place.x - loc.x))
effective_dist = dist - claw_offset
self.turn(angle, **kwargs)
if effective_dist < 0:
# The driver API skips the turn state if the request distance is
# negative. This ensures the rover will perform the turn before
# backing up slightly in this case.
return self.drive(effective_dist, **kwargs)
req = MoveRequest(
theta=angle,
r=effective_dist,
)
return self.__drive(req, **kwargs)
def polar_ctrl_avoid(position, ala_position, destination, K_polar):
# Polar coordinate control function with obstacle avoidance
# Local constants
radius = 0.3
threshold = 1
# remaping input variables
x = destination.point.x
y = destination.point.y
theta = position.theta
theta_goal = round(destination.point.z, 3)
x_ala = ala_position.point.x
y_ala = ala_position.point.y
# Remaping the gain variables
k_rho = K_polar[0]
k_alpha = K_polar[1]
k_beta = K_polar[2]
k_ala_v = K_polar[3]
k_ala_w = K_polar[4]
# Computing the coordinate transformation
rho = math.sqrt(x**2 + y**2)
alpha = math.atan2(y, x)
beta = round(angles.shortest_angular_distance(theta, theta_goal), 2)
# Computing the avoidance parameter
ala = math.sqrt(x_ala**2 + y_ala**2) - radius
alpha_ala = (math.atan2(y_ala, x_ala))
if alpha_ala > 0:
alpha_ala -= (math.pi / 2)
elif alpha_ala < 0:
alpha_ala += (math.pi / 2)
else:
pass
avoid = 1 / ((ala)**3 - (ala**2 * threshold))
if avoid < 0: avoid = 0
# Defining the position threshold
if abs(rho) < 0.15:
alpha = 0
rho = 0
# Computing the control law
v = (k_rho * rho)
w = (k_alpha * alpha) + (k_beta * beta)
# Avoidance action theshold
if ala <= threshold:
v = abs((k_rho * rho) - (ala * k_ala_v))
w = (k_alpha * alpha) + (k_beta * beta) + (k_ala_w * (alpha_ala))
agl = [rho, alpha, beta, avoid, ala, alpha_ala]
U_uni = [v, w, agl]
return U_uni
def test_normalize_angle_positive(self, a):
a = a * np.pi
ref_r = normalize_angle_positive(a)
sw_r = w.compile_and_execute(w.normalize_angle_positive, [a])
self.assertAlmostEqual(shortest_angular_distance(ref_r, sw_r),
0.0,
places=5)
def on_timer(self, event):
stamp = event.current_real or rospy.Time.now()
is_voice = self.respeaker.is_voice()
doa_rad = math.radians(self.respeaker.direction - 180.0)
doa_rad = angles.shortest_angular_distance(
doa_rad, math.radians(self.doa_yaw_offset))
doa = math.degrees(doa_rad)
# vad
if is_voice != self.prev_is_voice:
self.pub_vad.publish(Bool(data=is_voice))
self.prev_is_voice = is_voice
# doa
if doa != self.prev_doa:
self.pub_doa_raw.publish(Int32(data=doa))
self.prev_doa = doa
msg = PoseStamped()
msg.header.frame_id = self.sensor_frame_id
msg.header.stamp = stamp
ori = T.quaternion_from_euler(math.radians(doa), 0, 0)
msg.pose.position.x = self.doa_xy_offset * np.cos(doa_rad)
msg.pose.position.y = self.doa_xy_offset * np.sin(doa_rad)
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
self.pub_doa.publish(msg)
if self.asr_engine != "silent":
#actual cutting of the "speech" only audio for sending to asr service
# get timestamp of "last time mic array detected voice"
if is_voice and not self.pepper_is_speaking:
self.speech_stopped = stamp
#pepper is speaking: we go to cut (correct)
#pepper is not speaking:
#we are past the grace period in speaking:
#second statement fails, we go to cutting (correct)
#we are not past the grace period in speaking:
#second statement is true, we don't cut off audio and go on with is_speaking = True (correct)
if not self.pepper_is_speaking and (stamp - self.speech_stopped <
rospy.Duration(
self.speech_continuation)):
self.is_speaking = True #grace period "speech_continuation where we just continue 'is speaking'"
elif self.is_speaking: #otherwise there has been no speech for too long, and we were obviously speaking, so we cut the audio here
buf = self.speech_audio_buffer
self.speech_audio_buffer = str()
self.is_speaking = False
duration = 8.0 * len(buf) * self.respeaker_audio.bitwidth
duration = duration / self.respeaker_audio.rate / self.respeaker_audio.bitdepth
rospy.loginfo("Speech detected for %.3f seconds" % duration)
if self.speech_min_duration <= duration < self.speech_max_duration:
self.pub_speech_audio.publish(AudioData(data=buf))
def read_bag(bag):
pos = []
ori = []
t = []
table_poses = []
for topic, msg, _ in bag.read_messages('/tf'):
if msg.transforms[0].child_frame_id == "Puck":
pos.append([
msg.transforms[0].transform.translation.x,
msg.transforms[0].transform.translation.y,
msg.transforms[0].transform.translation.z
])
ori.append([
msg.transforms[0].transform.rotation.x,
msg.transforms[0].transform.rotation.y,
msg.transforms[0].transform.rotation.z,
msg.transforms[0].transform.rotation.w
])
t.append(msg.transforms[0].header.stamp.to_sec())
elif msg.transforms[0].child_frame_id == "Table":
# count_table += 1
pose_i = np.array([
msg.transforms[0].transform.translation.x,
msg.transforms[0].transform.translation.y,
msg.transforms[0].transform.translation.z,
msg.transforms[0].transform.rotation.x,
msg.transforms[0].transform.rotation.y,
msg.transforms[0].transform.rotation.z,
msg.transforms[0].transform.rotation.w
])
if len(table_poses) == 0 or not np.equal(
np.linalg.norm(table_poses[-1][2] - pose_i[2]), 0):
table_poses.append(pose_i)
ori = np.array(ori) # original orientation
# quaternion 2 euler
for i, quat in enumerate(ori):
ori[i, :3] = tf.transformations.euler_from_quaternion(quat)
ori = ori[:, -2]
# eliminate angle jump
elimated_ori = np.zeros((ori.shape[0])) # reduce to 1 dim
for i, o in enumerate(ori):
if i == 0:
elimated_ori[i] = o
else:
elimated_ori[i] = shortest_angular_distance(
ori[i - 1], ori[i]) + elimated_ori[i - 1]
ori = np.array(ori)
bagdata = np.zeros((len(t), 7)) # [t * 1, pos * 3, ang * 3]
bagdata[:, 0] = t
bagdata[:, 1:4] = np.array(pos)
# bagdata[:, -1] = elimated_ori
bagdata[:, -1] = ori
return bagdata, table_poses
def rotate():
global robot_fsm, active_goal, theta, goal, angle_to_goal_filt, theta_filt, angle_to_goal_prev, theta_prev
# Used for the velocity command storage
velocity = Twist()
# Calculate and unwrap angle to goal
angle_to_goal = degrees(
atan2(active_goal.y - cur_pos.y, active_goal.x - cur_pos.x))
#angle_to_goal = GOAL_THETA # Debug option: Uncomment for rotation parameters tuning
unwrapped = np.unwrap(
[radians(angle_to_goal_prev),
radians(angle_to_goal)])
angle_to_goal_uw = degrees(unwrapped[1])
angle_to_goal_prev = angle_to_goal_uw
# Unwrap theta
unwrapped = np.unwrap([radians(theta_prev), radians(theta)])
theta_uw = degrees(unwrapped[1])
theta_prev = theta_uw
# Filter readings and reference
theta_filt = P_ANG_THT * theta_filt + (1 - P_ANG_THT) * theta_uw
angle_to_goal_filt = P_ANG_DST * angle_to_goal_filt + (
1 - P_ANG_DST) * angle_to_goal_uw
# Positive - CW, Negative - CCW
angle_error = degrees(
shortest_angular_distance(radians(theta_filt),
radians(angle_to_goal_filt)))
# Calculate velocity inputs
if abs(angle_error) > angle_err_tolerance_rot:
speed_calc = rot_pid.pid_incremental(angle_error)
velocity.angular.z = speed_calc
else:
velocity.angular.z = 0
rot_pid.reset_previous()
robot_fsm.switch_state(StateForward)
#robot_fsm.switch_state(StateIdle) # Can be used for debugging purposes
# Publish command
pub_cmd_vel.publish(velocity)
# Debug publish
if debug_topics_enabled:
pub_dbg_angle_err.publish(angle_error)
pub_dbg_theta.publish(theta)
pub_dbg_theta_filtr.publish(theta_filt)
pub_dbg_theta_uw.publish(theta_uw)
pub_dbg_ang_to_goal.publish(angle_to_goal)
pub_dbg_ang_to_goal_filtr.publish(angle_to_goal_filt)
pub_dbg_ang_to_goal_uw.publish(angle_to_goal_uw)
pub_dbg_rot.publish(velocity.angular.z)
print("[ROT] Angle to goal: {}, theta: {}".format(
angle_to_goal, theta))
def __call__(self, god_map):
"""
:param god_map:
:type god_map: giskardpy.god_map.GodMap
:return:
:rtype: float
"""
a1 = god_map.get_data(self.current_angle) # type: float
a2 = god_map.get_data(self.goal_angle) # type: float
return shortest_angular_distance(a1, a2)
def check_joint_state(self, expected):
current_joint_state = self.get_current_joint_state()
for i, joint_name in enumerate(current_joint_state.name):
if joint_name in expected:
goal = expected[joint_name]
current = current_joint_state.position[i]
if self.robot.is_joint_continuous(joint_name):
np.testing.assert_almost_equal(
shortest_angular_distance(goal, current), 0)
else:
np.testing.assert_almost_equal(goal, current, 2)
def polar_control(robot_pose,
goal,
K_rho,
K_alpha,
K_beta,
max_lin=0.5,
max_ang=0.5):
"""
This function computes the control signal to guides the
robot to the desired goal. It's based on the Cartesian
Control Algorithm
"""
# recovering the variables
x = robot_pose.x
y = robot_pose.y
theta = robot_pose.theta
x_d = goal.x
y_d = goal.y
theta_d = goal.theta
# Computing the position error
delta_x = x_d - x
delta_y = y_d - y
heading = math.atan2(delta_y, delta_x)
# Creating the new variables
rho = round(math.sqrt(delta_x**2 + delta_y**2), 2)
alpha = round(angles.shortest_angular_distance(theta, heading), 2)
beta = round(-theta - alpha + theta_d, 2)
#beta = round(angles.shortest_angular_distance(heading,theta_d),2)
# Computing control signals
#v = round(K_rho*rho,3)
# Non-Linear control for rho
v = round(K_rho * rho * math.cos(alpha), 2)
omega = round(K_alpha * alpha + K_beta * beta, 2)
print('Rho:', rho, '\nAlpha:', alpha, '\nBeta:', beta)
# velocity limitation
v = max_lin * np.sign(v) if abs(v) > max_lin else v
omega = max_ang * np.sign(omega) if abs(omega) > max_ang else omega
u = Twist()
u.linear.x = v
u.angular.z = omega
return u
def pose_callback(cur_pose):
lock.acquire()
x = goal.x - cur_pose.x
y = goal.y - cur_pose.y
lock.release()
dist_to_goal = math.sqrt(x**2 + y**2)
error_angle = angles.shortest_angular_distance(angles.normalize_angle(cur_pose.theta), math.atan2(y, x))
velocity = Twist()
if abs(dist_to_goal) > 0.05:
velocity.linear.x = kf * dist_to_goal * math.cos(error_angle)
velocity.angular.z = kr * error_angle
vel_pub.publish(velocity)
def move2goal(self, goal):
self.pose_goal.x = goal.x
self.pose_goal.y = goal.y
self.pose_goal.theta = goal.theta
command = Twist()
integral_rotate_z = 0
while self.euclidean_distance(self.pose_goal, self.pose) >= self.dist_tolerance:
if min(self.scan_filter) < self.laser_clearance:
command.linear.x = 0.0
command.angular.z = 0.0
rospy.loginfo("Laser Dis")
else:
error_x = self.pose_goal.x - self.pose.x
error_y = self.pose_goal.y - self.pose.y
error_th = atan2(error_y, error_x)
error_th = angles.shortest_angular_distance(
self.pose.theta, error_th)
integral_rotate_z += error_th
if abs(error_th) > 0.5:
command.linear.x = 0.2
command.angular.z = 0.45 * \
(error_th) + 0.01 * integral_rotate_z
if abs(error_th) > 0.48:
integral_rotate_z = 0
else:
command.linear.x = 0.4
command.angular.z = 0.40 * \
(error_th) + 0.02 * integral_rotate_z
# integral_rotate_z = 0
# command.linear.x = 0.25
# command.angular.z = 0.70 * (error_th) #+ 0.05 * integral_rotate_z
txt = "{},{}".format(self.euclidean_distance(
self.pose_goal, self.pose), command.angular.z)
rospy.loginfo(txt)
self.pub_cmd_vel.publish(command)
self.rate.sleep()
command.linear.x = 0.0
command.angular.z = 0.0
self.pub_cmd_vel.publish(command)
rospy.loginfo("Goal Reach")
def pose_callback(cur_pose):
lock.acquire()
x = goal.x - cur_pose.x
y = goal.y - cur_pose.y
lock.release()
dist_to_goal = math.sqrt(x**2 + y**2)
error_angle = angles.shortest_angular_distance(
angles.normalize_angle(cur_pose.theta), math.atan2(y, x))
velocity = Twist()
if abs(dist_to_goal) > 0.05:
velocity.linear.x = kf * dist_to_goal * math.cos(error_angle)
velocity.angular.z = kr * error_angle
vel_pub.publish(velocity)
def goToGoal(self):
self.goal_d = self.goal_distance(self.goal, self.pose)
if self.goal_d < 0.1:
goal_direction = self.goalYaw
else:
goal_direction = self.goal_direction(self.goal, self.pose)
self.new_heading = goal_direction if goal_direction > 0 else 2 * math.pi + goal_direction #when going to a self.goal location
self.new_heading = angles.normalize_angle(self.new_heading)
t_amt = angles.shortest_angular_distance(self.yaw, self.new_heading)
if abs(t_amt) > self.tolerance:
self.turn = True
return t_amt
def turtle_command(turtle_odom, goal, gain):
print("Entrei em turtle_command")
print(turtle_odom)
print(goal)
#print(gain)
#Coordenadas da pose atual da tartaruga
x = turtle_odom.x
y = turtle_odom.y
theta = turtle_odom.theta
#Coordenadas de destino da tartaruga
x_d = goal.x
y_d = goal.y
theta_d = goal.theta
#Ganhos
#K_v = gain[0]
#K_w = gain[1]
#K_i = gain[2]
K_rho = gain[3]
K_alfa = gain[4]
K_beta = gain[5]
#Calculando diferença entre distancia de destino e atual (slide 16 - pdf 'controle de veículos')
e_x = x_d - x
e_y = y_d - y
#Calculando coordenadas polares para regulação de postura (slide 20,21,22,24 - pdf 'controle de veículos')
rho = round(math.sqrt(e_x**2 + e_y**2), 3) #distância até o destino
arco_tan = round(math.atan2(e_y, e_x),
3) #calculo de arco tangente do heading
#alfa = round(-theta+arco_tan,3) #direção da trejetória (heading)
alfa = angles.shortest_angular_distance(theta, arco_tan)
beta = -round(theta, 3) - round(alfa, 3) + round(theta_d,
3) #orientação final
#Calculando sinal de controle para velocidade linear (slide 24 - pdf 'controle de veículos')
v = K_rho * rho
#Calculando sinal de controle para orientação (velocidade angular)
w = K_alfa * alfa + K_beta * beta
#Construindo o objeto para publicação
turtle_vel = Twist()
turtle_vel.linear.x = v
turtle_vel.angular.z = w
return turtle_vel
def set_heading(self, heading, **kwargs):
'''Turn to face an absolute heading in radians. (zero is east)
Arguments:
* `heading`: (`float`) The heading in radians.
Keyword Arguments/Returns/Raises:
* See `mobility.swarmie.Swarmie.drive`
'''
loc = self.get_odom_location().get_pose()
angle = angles.shortest_angular_distance(loc.theta, heading)
self.turn(angle, **kwargs)
def get_gps_angle_and_dist():
global swarmie
# Use GPS to figure out about where we are.
# FIXME: We need to hanlde poor GPS fix.
loc = swarmie.wait_for_fix(distance=4, time=60).get_pose()
home = swarmie.get_home_gps_location()
dist = math.hypot(loc.y - home.y, loc.x - home.x)
angle = angles.shortest_angular_distance(
loc.theta, math.atan2(home.y - loc.y, home.y - loc.x))
# swarmie.turn(angle, ignore=Obstacle.TAG_TARGET | Obstacle.SONAR_CENTER)
# swarmie.drive(dist, ignore=Obstacle.TAG_TARGET | Obstacle.SONAR_CENTER)
return angle, dist
def compute_velociy_commands(self):
"""Compute velocity commands
Returns:
bool: True if velocity is computed, False otherwise
Twist: Computed velocity command
"""
map_pose, odom_pose = self.robot.get_pose()
if map_pose == None:
rospy.logerr("Robot pose could not retreived")
return False, None
if self.goal_reached(map_pose.pose):
return True, self.get_twist(0.0, 0.0)
if self.goal_path_ind == None:
self.goal_path_ind, self.local_plan = self.get_local_plan(self.robot_path_ind)
elif self.robot_path_ind == self.goal_path_ind:
self.goal_path_ind, self.local_plan = self.get_local_plan(self.robot_path_ind)
goal = self.global_plan.poses[self.robot_path_ind].pose
dist = self.calc_distance(map_pose.pose, goal)
yaw = tft.euler_from_quaternion([map_pose.pose.orientation.x, map_pose.pose.orientation.y, map_pose.pose.orientation.z, map_pose.pose.orientation.w])[2]
yaw = yaw
a = self.calc_angle(map_pose.pose, goal)
print("Angle: %f - Yaw: %f"%(a, yaw))
angle = angles.shortest_angular_distance(yaw, a)
if(dist < 0.3):
self.robot_path_ind += 1
self.fuzzy_control_sim.input['distance'] = dist
self.fuzzy_control_sim.input['angle'] = angle
self.fuzzy_control_sim.compute()
wl = self.fuzzy_control_sim.output['wl']
wr = self.fuzzy_control_sim.output['wr']
rospy.logwarn("Distance: %f Angle: %f Wl: %f Wr: %f"%(dist, angle, wl, wr))
rospy.logerr("Goal: (%f, %f) Robot: (%f, %f)"%(goal.position.x, goal.position.y, map_pose.pose.position.x, map_pose.pose.position.y))
return True, self.get_twist(wl, wr)
def on_timer(self, event):
stamp = event.current_real or rospy.Time.now()
is_voice = self.respeaker.is_voice()
doa_rad = math.radians(self.respeaker.direction - 180.0)
doa_rad = angles.shortest_angular_distance(
doa_rad, math.radians(self.doa_yaw_offset))
doa = math.degrees(doa_rad)
# vad
if is_voice != self.prev_is_voice:
self.pub_vad.publish(Bool(data=is_voice))
self.prev_is_voice = is_voice
# doa
if doa != self.prev_doa:
self.pub_doa_raw.publish(Int32(data=doa))
self.prev_doa = doa
msg = PoseStamped()
msg.header.frame_id = self.sensor_frame_id
msg.header.stamp = stamp
ori = T.quaternion_from_euler(math.radians(doa), 0, 0)
msg.pose.position.x = self.doa_xy_offset * np.cos(doa_rad)
msg.pose.position.y = self.doa_xy_offset * np.sin(doa_rad)
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
self.pub_doa.publish(msg)
# speech audio
if is_voice:
self.speech_stopped = stamp
if stamp - self.speech_stopped < rospy.Duration(
self.speech_continuation):
self.is_speeching = True
elif self.is_speeching:
buf = self.speech_audio_buffer
self.speech_audio_buffer = str()
self.is_speeching = False
duration = 8.0 * len(buf) * self.respeaker_audio.bitwidth
duration = duration / self.respeaker_audio.rate / self.respeaker_audio.bitdepth
rospy.loginfo("Speech detected for %.3f seconds" % duration)
if self.speech_min_duration <= duration < self.speech_max_duration:
self.pub_speech_audio.publish(AudioData(data=buf))
def on_timer(self, event):
stamp = event.current_real or rospy.Time.now()
is_voice = self.respeaker.is_voice()
doa_rad = math.radians(self.respeaker.direction - 180.0)
doa_rad = angles.shortest_angular_distance(
doa_rad, math.radians(self.doa_yaw_offset))
doa = math.degrees(doa_rad)
# vad
if is_voice != self.prev_is_voice:
self.pub_vad.publish(Bool(data=is_voice))
self.prev_is_voice = is_voice
# doa
if doa != self.prev_doa:
self.pub_doa_raw.publish(Int32(data=doa))
self.prev_doa = doa
msg = PoseStamped()
msg.header.frame_id = self.sensor_frame_id
msg.header.stamp = stamp
ori = T.quaternion_from_euler(math.radians(doa), 0, 0)
msg.pose.position.x = self.doa_xy_offset * np.cos(doa_rad)
msg.pose.position.y = self.doa_xy_offset * np.sin(doa_rad)
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
self.pub_doa.publish(msg)
# speech audio
if is_voice:
self.speech_stopped = stamp
if stamp - self.speech_stopped < rospy.Duration(self.speech_continuation):
self.is_speeching = True
elif self.is_speeching:
buf = self.speech_audio_buffer
self.speech_audio_buffer = str()
self.is_speeching = False
duration = 8.0 * len(buf) * self.respeaker_audio.bitwidth
duration = duration / self.respeaker_audio.rate / self.respeaker_audio.bitdepth
rospy.loginfo("Speech detected for %.3f seconds" % duration)
if self.speech_min_duration <= duration < self.speech_max_duration:
self.pub_speech_audio.publish(AudioData(data=buf))
def tracking_control(donkey_pose, master_pose, delta_tracking, gains,
error_int, max_vel):
"""
This function computes the control signal to guides the
robot to the desired goal. It's based on the Cartesian
Control Algorithm
"""
# Recovering control gains
K_v = gains[0]
K_int = gains[1]
K_omega = gains[2]
# Recovering velocitie limits
max_lin_vel = max_vel[0]
max_ang_vel = max_vel[1]
# Computing the position error
error_x = master_pose.x - donkey_pose.x
error_y = master_pose.y - donkey_pose.y
error_lin = round(math.sqrt(error_x**2 + error_y**2) - delta_tracking, 3)
error_int += error_lin * 0.066
# Computing the heading
heading = math.atan2(error_y, error_x)
error_th = round(
angles.shortest_angular_distance(donkey_pose.theta, heading), 3)
errors = [error_int, error_lin, error_th]
# Computing control signals
v = K_v * error_lin + K_int * error_int
v = np.sign(v) * max_lin_vel if v > max_lin_vel else v
omega = K_omega * error_th
omega = np.sign(omega) * max_ang_vel if omega > max_ang_vel else omega
u = Twist()
u.linear.x = v
u.angular.z = omega
return u, errors
def test_shortest_angular_distance(self):
self.assertAlmostEqual(pi/2, shortest_angular_distance(0, pi/2))
self.assertAlmostEqual(-pi/2, shortest_angular_distance(0, -pi/2))
self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi/2, 0))
self.assertAlmostEqual(pi/2, shortest_angular_distance(-pi/2, 0))
self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi, pi/2))
self.assertAlmostEqual(pi/2, shortest_angular_distance(pi, -pi/2))
self.assertAlmostEqual(pi/2, shortest_angular_distance(pi/2, pi))
self.assertAlmostEqual(-pi/2, shortest_angular_distance(-pi/2, pi))
self.assertAlmostEqual(-pi/2, shortest_angular_distance(5*pi, pi/2))
self.assertAlmostEqual(pi/2, shortest_angular_distance(7*pi, -pi/2))
self.assertAlmostEqual(pi/2, shortest_angular_distance(9*pi/2, pi))
self.assertAlmostEqual(pi/2, shortest_angular_distance(-3*pi/2, pi))
# Backside wrapping
self.assertAlmostEqual(-pi/2, shortest_angular_distance(-3*pi/4, 3*pi/4))
self.assertAlmostEqual(pi/2, shortest_angular_distance(3*pi/4, -3*pi/4))