def update_position(self):
# Transform from base_laser_link to base_link
tf_target = self.tf_listener.transformPoint('base_link',
self.target_pos)
# tf_target = self.target_pos
# print(tf_target)
# If the angle is not aligned, turn. Otherwise, go forward.
error = sqrt(tf_target.point.x**2 + tf_target.point.y**2)
# print("x, y = {}, {}\terror = {}".format(self.target_pos.linear.x, self.target_pos.linear.y, error))
# new_cmd_vel = self.k_p * error * cos(self.target_pos.angular.z)
# angular_error = self.target_pos.angular.z
# angular_error = error * cos(self.target_pos.angular.z)
# angular_error = atan2(tf_target.point.y, tf_target.point.x) % (math.pi * 2)
# angular_error = atan2(self.target_pos.point.y, self.target_pos.point.x)
# angular_error = self.target_pos.point.z
if (abs(tf_target.point.y) < 0.3):
# Allow some forward speed
new_cmd_vel = self.max_linear_speed
new_cmd_angle = 0
else:
new_cmd_vel = 0
new_cmd_angle = self.max_angular_speed
# Project the vector to the target point onto the forward unit vector of the robot.
# This way, if the target point is behind or in a different direction, more weight will
# be given to the angular compensation and the robot will slow down.
# error = sqrt(self.target_pos.linear.x**2 + self.target_pos.linear.y**2) - self.distance
# print("x, y = {}, {}\terror = {}".format(self.target_pos.linear.x, self.target_pos.linear.y, error))
# new_cmd_vel = self.k_p * error * cos(self.target_pos.angular.z)
# new_cmd_angle = 1.0 * self.k_p * self.target_pos.angular.z * self.max_angular_speed
print("new_cmd_vel = {}, new_cmd_angle = {}".format(
round(new_cmd_vel, 2), round(new_cmd_angle, 2)))
# print("angular_error = {}, laser_link angle = {}".format(round(degrees(angular_error), 2),round(degrees(self.target_pos.point.z))))
print("x_component = {}, y_component = {}".format(
tf_target.point.x, tf_target.point.y))
marker_target = Marker()
marker_target.header.frame_id = "base_laser_link"
marker_target.type = Marker.ARROW
marker_target.points = [
Vector3(0, 0, 0),
Vector3(self.target_pos.point.x, self.target_pos.point.y, 0)
]
marker_target.scale = Vector3(0.1, 0.4, 0.1)
marker_target.color.r = 1
marker_target.color.a = 0.5
# publish the new target point
self.target_marker_publisher.publish(marker_target)
new_polar_vel = LabeledPolarVelocity2D(node_ID=State.FOLLOW,
velocity=PolarVelocity2D(
linear=new_cmd_vel,
angular=new_cmd_angle))
# TODO: should I be publishing to a different place?
self.cmd_vel_publisher.publish(new_polar_vel)
def estop(self, bump_msg):
# If any of the bump sensors are depressed, STAHP.
if (bump_msg.leftSide or bump_msg.leftFront or bump_msg.rightSide
or bump_msg.rightFront):
print("EMERGENCY. STAHHHHP")
self.cmd_vel_publisher.publish(
LabeledPolarVelocity2D(node_ID=State.ESTOP,
velocity=PolarVelocity2D(linear=0,
angular=0)))
self.state_publisher.publish(State.ESTOP)
def square_side(self):
# Move forwards at 1 m/s
print("Move forward")
cmd_vel = LabeledPolarVelocity2D(
node_ID=self.behavior_id,
velocity=PolarVelocity2D(linear=1, angular=0))
self.publisher_cmd_vel.publish(cmd_vel)
# Wait 1s
rospy.sleep(self.forward_wait_time)
# Stop and turn
print("Turn")
cmd_vel = LabeledPolarVelocity2D(
node_ID=self.behavior_id,
velocity=PolarVelocity2D(linear=0, angular=10))
self.publisher_cmd_vel.publish(cmd_vel)
# Wait 1s
rospy.sleep(self.turn_wait_time)
def update_position(self):
# The robot is positioned at (0, 0). If the angle of the target is no self.angle,
# there is an error.
error = self.angle - self.target_pos.angular.z
print("error is: {}".format(error))
# If the error is 0, the robot is at the correct angle. The new_cmd_angle should be 0.
# As the error gets smaller, the slower the angular speed becomes.
new_cmd_angle = min(abs(self.k_p * error * self.max_angular_speed), self.max_angular_speed)
new_polar_vel = LabeledPolarVelocity2D(node_ID=self.behavior_id, velocity=PolarVelocity2D(linear=0, angular=new_cmd_angle))
print("I want to turn this much: {}".format(new_cmd_angle))
# TODO: should I be publishing to a different place?
self.cmd_vel_publisher.publish(new_polar_vel)
def __init__(self):
rospy.init_node("warmup_nav_arbiter_node")
self.desired_vel_subscriber = rospy.Subscriber(
"/desired_cmd_vel", LabeledPolarVelocity2D,
self.update_current_request)
self.desired_state_subscriber = rospy.Subscriber(
"/desired_state", State, self.update_state)
self.cmd_vel_publisher = rospy.Publisher("/cmd_vel",
Twist,
queue_size=10,
latch=True)
self.state = State.TELEOP
self.current_requesting_node = None
self.desired_vel = PolarVelocity2D(linear=0, angular=0)
def follow_wall(self, scan_msg):
"""
Callback function for /scan subscription.
Uses proportional control to stay a certain distance from a wall.
"""
max_side_angle = 65
baseline_angular_speed = 1
top_angular_speed = 2
linear_speed = 1
desired_distance_to_wall = 1
k_p = 10
# TODO: clamp the error: if it is below like 0.02, it's probably parallel enough
distance_estimate = self.estimate_distance_to_wall(
scan_msg.ranges, max_side_angle)
angle_error = self.get_angular_error(scan_msg.ranges, max_side_angle)
if distance_estimate is None:
distance_error = 0
else:
distance_error = distance_estimate - desired_distance_to_wall
print("distance_error = {}".format(distance_error))
if (angle_error != None):
error = angle_error + distance_error
angular_speed = min(k_p * error * baseline_angular_speed,
top_angular_speed)
# publish a new cmd_vel
new_cmd_vel = LabeledPolarVelocity2D(node_ID=self.behavior_id,
velocity=PolarVelocity2D(
linear=linear_speed,
angular=angular_speed))
self.cmd_vel_publisher.publish(new_cmd_vel)
# TODO: Delete when above has been tested
# # publish a new cmd_vel
# angular_speed = min(k_p * error * baseline_angular_speed, top_angular_speed)
# new_cmd_vel = Twist(linear=Vector3(linear_speed, 0, 0), angular=Vector3(0, 0, angular_speed))
# print("New angular vel = {}".format(k_p * error * baseline_angular_speed))
# self.cmd_vel_publisher.publish(new_cmd_vel)
print()
# Define mapping of command names to robot states
command_to_state = {"estop": State.ESTOP, "wall": State.WALL_FOLLOW}
key_actions = {
'a': 'MOVE_LEFT',
'd': 'MOVE_RIGHT',
'w': 'MOVE_FORWARD',
's': 'MOVE_BACKWARD',
'z': "TURN_LEFT",
'x': "TURN_RIGHT",
'0': "E_STOP"
}
action_bindings = {
'MOVE_LEFT': PolarVelocity2D(linear=0.5, angular=1),
'MOVE_RIGHT': PolarVelocity2D(linear=0.5, angular=-1),
'MOVE_FORWARD': PolarVelocity2D(linear=1, angular=0),
'MOVE_BACKWARD': PolarVelocity2D(linear=-1, angular=0),
"TURN_LEFT": PolarVelocity2D(linear=0, angular=1),
"TURN_RIGHT": PolarVelocity2D(linear=0, angular=-1),
"E_STOP": PolarVelocity2D(linear=0, angular=0)
}
class TeleopNode(object):
def __init__(self):
# initialize teleop node
rospy.init_node("neato_teleop")
# TODO: to make this standalone, change the topic to be published to based on whether is launches standalone or not.
self.cmd_vel_publisher = rospy.Publisher("/desired_cmd_vel",