def ArenaState_callback(self, arenastate):
if self.initialized:
robot_x = arenastate.robot.pose.position.x
robot_y = arenastate.robot.pose.position.y
try:
fly_x = arenastate.flies[0].pose.position.x
fly_y = arenastate.flies[0].pose.position.y
except:
return
# Get and publish InBounds.
robot_dist = N.sqrt((robot_x - self.start_position_x) ** 2 + (robot_y - self.start_position_y) ** 2)
fly_dist = N.sqrt((fly_x - self.start_position_x) ** 2 + (fly_y - self.start_position_y) ** 2)
self.in_bounds.bounds_radius = self.radiusArena
if robot_dist < self.radiusArena:
self.in_bounds.robot_in_bounds = True
else:
self.in_bounds.robot_in_bounds = False
if fly_dist < self.radiusArena:
self.in_bounds.fly_in_bounds = True
else:
self.in_bounds.fly_in_bounds = False
self.pubInBounds.publish(self.in_bounds)
# Get & publish the FlyView.
try:
self.tfrx.waitForTransform("Fly1", self.robot_origin.header.frame_id, rospy.Time(), rospy.Duration(5.0))
self.robot_origin_fly_frame = self.tfrx.transformPoint("Fly1", self.robot_origin)
self.fly_view.robot_position_x = rx = self.robot_origin_fly_frame.point.x
self.fly_view.robot_position_y = ry = self.robot_origin_fly_frame.point.y
self.fly_view.robot_angle = CircleFunctions.mod_angle(N.arctan2(ry, rx))
self.fly_view.robot_distance = N.sqrt(rx ** 2 + ry ** 2)
self.pubFlyView.publish(self.fly_view)
except (tf.Exception):
pass
def ArenaState_callback(self, arenastate):
if self.initialized:
robot_x = arenastate.robot.pose.position.x
robot_y = arenastate.robot.pose.position.y
try:
fly_x = arenastate.flies[0].pose.position.x
fly_y = arenastate.flies[0].pose.position.y
except:
return
# Get and publish InBounds.
robot_dist = np.sqrt((robot_x - self.start_position_x)**2 +
(robot_y - self.start_position_y)**2)
fly_dist = np.sqrt((fly_x - self.start_position_x)**2 +
(fly_y - self.start_position_y)**2)
self.in_bounds.bounds_radius = self.radiusArena
if robot_dist < self.radiusArena:
self.in_bounds.robot_in_bounds = True
else:
self.in_bounds.robot_in_bounds = False
if fly_dist < self.radiusArena:
self.in_bounds.fly_in_bounds = True
else:
self.in_bounds.fly_in_bounds = False
self.pubInBounds.publish(self.in_bounds)
# Get & publish the FlyView.
try:
self.tfrx.waitForTransform("Fly1",
self.robot_origin.header.frame_id,
rospy.Time(), rospy.Duration(5.0))
self.robot_origin_fly_frame = self.tfrx.transformPoint(
"Fly1", self.robot_origin)
self.fly_view.robot_position_x = rx = self.robot_origin_fly_frame.point.x
self.fly_view.robot_position_y = ry = self.robot_origin_fly_frame.point.y
self.fly_view.robot_angle = CircleFunctions.mod_angle(
np.arctan2(ry, rx))
self.fly_view.robot_distance = np.sqrt(rx**2 + ry**2)
self.pubFlyView.publish(self.fly_view)
except (tf.Exception):
pass
def joystick_commands_callback(self,data):
if self.initialized:
self.control_frame = data.header.frame_id
self.setpoint.header.frame_id = self.control_frame
self.setpoint_center_origin.header.frame_id = self.control_frame
self.setpoint_origin.header.frame_id = self.control_frame
self.setpoint_int_origin.header.frame_id = self.control_frame
self.setpoint.radius += self.inc_radius*data.radius_inc
if self.setpoint.radius < self.setpoint_radius_min:
self.setpoint.radius = self.setpoint_radius_min
elif self.setpoint_radius_max < self.setpoint.radius:
self.setpoint.radius = self.setpoint_radius_max
self.setpoint.theta += self.inc_theta*data.theta_inc
self.setpoint.theta = CircleFunctions.mod_angle(self.setpoint.theta)
# self.setpoint.theta = math.fmod(self.setpoint.theta,2*math.pi)
# if self.setpoint.theta < 0:
# self.setpoint.theta = 2*math.pi - self.setpoint.theta
if data.tracking and (not self.tracking):
self.tracking = True
if data.goto_start and (not self.goto_start):
self.goto_start = True
self.tracking = False
if data.stop:
self.goto_start = False
self.tracking = False
self.setpoint_pub.publish(self.setpoint)
self.setpoint_origin.point.x = self.setpoint.radius*math.cos(self.setpoint.theta)
self.setpoint_origin.point.y = self.setpoint.radius*math.sin(self.setpoint.theta)
self.setpoint_arena = self.convert_to_arena(self.setpoint_origin)
# rospy.logwarn("setpoint_origin.point.x = %s" % (str(self.setpoint_origin.point.x)))
# rospy.logwarn("setpoint_origin.point.y = %s" % (str(self.setpoint_origin.point.y)))
if not self.tracking:
if self.goto_start:
if not self.moving_to_start:
self.moving_to_start = True
self.stage_commands.position_control = True
self.stage_commands.velocity_control = False
self.stage_commands.lookup_table_correct = False
self.set_path_to_start(self.robot_velocity_max)
self.sc_ok_to_publish = True
self.goto_start = False
else:
self.sc_ok_to_publish = False
else:
self.sc_ok_to_publish = True
if self.moving_to_start:
self.moving_to_start = False
self.stage_commands.position_control = False
self.stage_commands.velocity_control = True
self.stage_commands.lookup_table_correct = False
self.radius_velocity = data.radius_velocity*self.robot_velocity_max
self.tangent_velocity = data.tangent_velocity*self.robot_velocity_max
self.vel_vector_arena[0,0] = data.x_velocity*self.robot_velocity_max
self.vel_vector_arena[1,0] = data.y_velocity*self.robot_velocity_max
try:
(self.stage_commands.x_velocity,self.stage_commands.y_velocity) = self.vel_vector_convert(self.vel_vector_arena,"Arena")
except (tf.LookupException, tf.ConnectivityException):
self.stage_commands.x_velocity = [self.vel_vector_arena[0,0]]
self.stage_commands.y_velocity = [-self.vel_vector_arena[1,0]]
if self.sc_ok_to_publish:
self.sc_pub.publish(self.stage_commands)
