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)