def twist_cb(self, msg): if self.cur_orientation is None or self.cur_position is None: return linear = mil_ros_tools.rosmsg_to_numpy(msg.linear) angular = mil_ros_tools.rosmsg_to_numpy(msg.angular) self.target_position += self.target_orientation.dot(self._position_gain * linear) self.diff_position = np.subtract(self.cur_position, self.target_position) gained_angular = self._orientation_gain * angular skewed = mil_ros_tools.skew_symmetric_cross(gained_angular) rotation = linalg.expm(skewed) # TODO: Better # self.target_orientation = self.cur_orientation # self.target_orientation = rotation.dot(self.target_orientation) self.target_orientation = self.target_orientation.dot(rotation) self.target_distance = round(np.linalg.norm(np.array([self.diff_position[0], self.diff_position[1]])), 3) self.target_depth = round(self.diff_position[2], 3) self.target_orientation = self.target_orientation.dot(rotation) blank = np.eye(4) blank[:3, :3] = self.target_orientation self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(blank) self.publish_target_pose(self.target_position, self.target_orientation_quaternion)
def run(self, args): fprint('Getting Guess Locations') sub_start_position, sub_start_orientation = yield self.tx_pose() gate_txros = yield self.nh.get_service_client('/guess_location', GuessRequest) gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1')) gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2')) gate_1 = mil_ros_tools.rosmsg_to_numpy( gate_1_req.location.pose.position) gate_2 = mil_ros_tools.rosmsg_to_numpy( gate_2_req.location.pose.position) mid = (gate_1 + gate_2) / 2 fprint('Found mid {}'.format(mid)) fprint('Looking at gate') yield self.move.down(DOWN).set_orientation(sub_start_orientation).go( speed=DOWN_SPEED) yield self.move.look_at_without_pitching(mid).go(speed=DOWN_SPEED) fprint('Going!') yield self.move.set_position(mid).depth(DOWN).go(speed=SPEED)
def set_pose_server(self, srv): '''Set the pose of the submarine TODO: Make this an 'abstract' method of Entity, and assign each new Entity a name/id ''' rospy.logwarn("Manually setting position of simulated vehicle") position = mil_ros_tools.rosmsg_to_numpy(srv.pose.position) self.body.setPosition(position) rotation_q = mil_ros_tools.rosmsg_to_numpy(srv.pose.orientation) rotation_norm = np.linalg.norm(rotation_q) velocity = mil_ros_tools.rosmsg_to_numpy(srv.twist.linear) angular = mil_ros_tools.rosmsg_to_numpy(srv.twist.angular) self.body.setLinearVel(velocity) self.body.setAngularVel(angular) # If the commanded rotation is valid if np.isclose(rotation_norm, 1., atol=1e-2): self.body.setQuaternion(mil_ros_tools.normalize(rotation_q)) else: rospy.logwarn( "Commanded quaternion was not a unit quaternion, NOT setting rotation" ) return SimSetPoseResponse()
def twist_cb(self, msg): if self.cur_orientation is None or self.cur_position is None: return linear = mil_ros_tools.rosmsg_to_numpy(msg.linear) angular = mil_ros_tools.rosmsg_to_numpy(msg.angular) self.target_position += self.target_orientation.dot( self._position_gain * linear) self.diff_position = np.subtract(self.cur_position, self.target_position) gained_angular = self._orientation_gain * angular skewed = mil_ros_tools.skew_symmetric_cross(gained_angular) rotation = linalg.expm(skewed) # TODO: Better # self.target_orientation = self.cur_orientation # self.target_orientation = rotation.dot(self.target_orientation) self.target_orientation = self.target_orientation.dot(rotation) self.target_distance = round( np.linalg.norm( np.array([self.diff_position[0], self.diff_position[1]])), 3) self.target_depth = round(self.diff_position[2], 3) self.target_orientation = self.target_orientation.dot(rotation) blank = np.eye(4) blank[:3, :3] = self.target_orientation self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix( blank) self.publish_target_pose(self.target_position, self.target_orientation_quaternion)
def run(self, args): fprint('Getting Guess Locations') sub_start_position, sub_start_orientation = yield self.tx_pose() gate_txros = yield self.nh.get_service_client('/guess_location', GuessRequest) gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1')) gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2')) gate_1 = mil_ros_tools.rosmsg_to_numpy( gate_1_req.location.pose.position) gate_2 = mil_ros_tools.rosmsg_to_numpy( gate_2_req.location.pose.position) #mid = (gate_1 + gate_2) / 2 #mid = gate_1 fprint('Found mid {}'.format(gate_1)) fprint('Looking at gate') # yield self.move.down(DOWN).set_orientation(sub_start_orientation).go( #########speed=DOWN_SPEED) # yield self.move.look_at_without_pitching(mid).go(speed=DOWN_SPEED) fprint('Going!') yield self.move.set_position(gate_1).depth(DOWN).go(speed=SPEED) yield self.move.forward(1).go(speed=SPEED)
def run(sub): print_info = text_effects.FprintFactory(title=MISSION).fprint print_bad = text_effects.FprintFactory(title=MISSION, msg_color="red").fprint print_good = text_effects.FprintFactory(title=MISSION, msg_color="green").fprint print_info("STARTING") # Set geometry of marker model print_info("SETTING GEOMETRY") polygon = RectFinder(LENGTH, WIDTH).to_polygon() yield sub.vision_proxies.orange_rectangle.set_geometry(polygon) # Wait for vision services, enable perception print_info("ACTIVATING PERCEPTION SERVICE") yield sub.vision_proxies.orange_rectangle.start() pattern = [] if DO_PATTERN: start = sub.move.zero_roll_and_pitch() r = SEARCH_RADIUS_METERS pattern = [start.zero_roll_and_pitch(), start.right(r), start.forward(r), start.left(r), start.backward(r), start.right(2 * r), start.forward(2 * r), start.left(2 * r), start.backward(2 * r)] s = Searcher(sub, sub.vision_proxies.orange_rectangle.get_pose, pattern) resp = None print_info("RUNNING SEARCH PATTERN") resp = yield s.start_search(loop=False, timeout=TIMEOUT_SECONDS, spotings_req=1) if resp is None or not resp.found: print_bad("MARKER NOT FOUND") defer.returnValue(None) print_good("PATH MARKER POSE FOUND") assert(resp.pose.header.frame_id == "/map") move = sub.move position = rosmsg_to_numpy(resp.pose.pose.position) position[2] = move._pose.position[2] # Leave Z alone! orientation = rosmsg_to_numpy(resp.pose.pose.orientation) move = move.set_position(position).set_orientation(orientation).zero_roll_and_pitch() # Ensure SubjuGator continues to face same general direction as before (doesn't go in opposite direction) odom_forward = tf.transformations.quaternion_matrix(sub.move._pose.orientation).dot(forward_vec) marker_forward = tf.transformations.quaternion_matrix(orientation).dot(forward_vec) if FACE_FORWARD and np.sign(odom_forward[0]) != np.sign(marker_forward[0]): move = move.yaw_right(np.pi) print_info("MOVING TO MARKER AT {}".format(move._pose.position)) yield move.go(speed=SPEED) print_good("ALIGNED TO PATH MARKER. MOVE FORWARD TO NEXT CHALLENGE!") yield sub.vision_proxies.orange_rectangle.stop() defer.returnValue(True)
def test_make_wrench_stamped(self): '''Test that wrenchmaking works''' for k in range(10): force = np.random.random(3) * 10 torque = np.random.random(3) * 10 wrench_msg = make_wrench_stamped(force, torque, frame='/enu') msg_force = rosmsg_to_numpy(wrench_msg.wrench.force) # noqa msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque) # noqa self.assertIsNotNone(msg_force) self.assertIsNotNone(msg_torque)
def test_make_wrench_stamped(self): '''Test that wrenchmaking works''' for k in range(10): force = np.random.random(3) * 10 torque = np.random.random(3) * 10 wrench_msg = make_wrench_stamped(force, torque, frame='/enu') msg_force = rosmsg_to_numpy(wrench_msg.wrench.force) # noqa msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque) # noqa self.assertIsNotNone(msg_force) self.assertIsNotNone(msg_torque)
def find_gate(self, objects, ray, min_distance_away=2.85, max_distance_away=3.3, perp_threshold=0.5, depth_threshold=1): ''' find_gate: search for two objects that satisfy critria Parameters: ray: direction we expect gate to be near min_distance_away: minimum distance for the two poles max_distance_away: max distance the two objects can be away from each other perp_threshold: max dot product value for perpindicular test with ray depth_threshold: make sure the two objects have close enough depth ''' for o in objects: p = rosmsg_to_numpy(o.pose.position) for o2 in objects: if o2 is o: continue p2 = rosmsg_to_numpy(o2.pose.position) if distance.euclidean(p, p2) > max_distance_away: fprint('Poles too far away. Distance {}'.format( distance.euclidean(p, p2))) continue if distance.euclidean(p, p2) < min_distance_away: fprint('Poles too close. Distance {}'.format( distance.euclidean(p, p2))) continue line = p - p2 perp = line.dot(ray) perp = perp / np.linalg.norm(perp) if not (-perp_threshold <= perp <= perp_threshold): fprint('Not perpendicular. Dot {}'.format(perp)) pass # continue print('Dist {}'.format(line)) if abs(line[2] > depth_threshold): print('Not similar height. Height: {}. Thresh: '.format( line[2], depth_threshold)) continue if abs(line[0]) < 1 and abs(line[1]) < 1: fprint('Objects on top of one another. x {}, y {}'.format( line[0], line[1])) continue return (p, p2) return None
def twist_cb(self, msg): linear = mil_ros_tools.rosmsg_to_numpy(msg.linear) angular = mil_ros_tools.rosmsg_to_numpy(msg.angular) if self.behavior == 'wrench': # Directly transcribe linear and angular 'velocities' to torque # TODO: Setup actual TF! self.wrench_pub.publish( mil_ros_tools.make_wrench_stamped( force=self.linear_gain * self.world_to_body.dot(linear), torque=self.angular_gain * self.world_to_body.dot(angular))) else: raise NotImplementedError
def test_rosmsg_to_numpy_quaternion(self): '''Test a rosmsg conversion for a geometry_msgs/Quaternion''' # I know this is not a unit quaternion q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2) numpy_array = rosmsg_to_numpy(q) np.testing.assert_allclose(np.array([0.7, 0.7, 0.1, 0.2]), numpy_array)
def transform_to_baselink(self, sub, pinger_1_req, pinger_2_req): transform = yield sub._tf_listener.get_transform('/base_link', '/map') position = yield sub.pose.position pinger_guess = [ transform._q_mat.dot( mil_ros_tools.rosmsg_to_numpy(x.location.pose.position) - position) for x in (pinger_1_req, pinger_2_req) ] for idx, guess in enumerate(pinger_guess): marker = Marker(ns='pinger', action=visualization_msgs.Marker.ADD, type=Marker.ARROW, scale=Vector3(0.1, 0.2, 0), points=np.array([ Point(0, 0, 0), Point(guess[0], guess[1], guess[2]) ])) marker.id = idx marker.header.frame_id = '/base_link' marker.color.r = 0 marker.color.g = 1 marker.color.a = 1 global markers markers.markers.append(marker) defer.returnValue(pinger_guess)
def twist_cb(self, msg): linear = mil_ros_tools.rosmsg_to_numpy(msg.linear) angular = mil_ros_tools.rosmsg_to_numpy(msg.angular) if self.behavior == 'wrench': # Directly transcribe linear and angular 'velocities' to torque # TODO: Setup actual TF! self.wrench_pub.publish( mil_ros_tools.make_wrench_stamped( force=self.linear_gain * self.world_to_body.dot(linear), torque=self.angular_gain * self.world_to_body.dot(angular) ) ) else: raise NotImplementedError
def test_rosmsg_to_numpy_custom(self): '''Test a rosmsg conversion for a custom msg''' pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14) numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta']) np.testing.assert_allclose( np.array([1.0, 2.0, 3.14]), numpy_array )
def test_rosmsg_to_numpy_vector(self): '''Test a rosmsg conversion for a geometry_msgs/Vector''' v = Vector3(x=0.1, y=99., z=21.) numpy_array = rosmsg_to_numpy(v) np.testing.assert_allclose( np.array([0.1, 99., 21.]), numpy_array )
def run(sub): yield sub.vision_proxies.start_gate.start() # Add search pattern if needed... search_pattern = [] search = Searcher(sub, sub.vision_proxies.start_gate.get_pose, search_pattern) resp = None fprint('Searching...') resp = yield search.start_search(loop=False, timeout=100, spotings_req=1) if resp is None or not resp.found: fprint("No gate found...", msg_color="red") defer.returnValue(None) position = rosmsg_to_numpy(resp.pose.pose.position) orientation = rosmsg_to_numpy(resp.pose.pose.orientation) fprint('Gate\'s position in map is: {}'.format(position)) fprint('Gate\'s orientation in map is: {}'.format(orientation)) # Get the normal vector, which is assumed to be the [1,0,0] unit vector normal = tf.transformations.quaternion_matrix(orientation).dot( np.array([1, 0, 0, 0]))[0:3] fprint('Computed normal vector: {}'.format(normal)) # Computer points before and after the gate for the sub to go to point_before = position + FACTOR_DISTANCE_BEFORE * normal point_after = position - FACTOR_DISTANCE_AFTER * normal # go in front of gate fprint('Moving infront of gate {}'.format(point_before)) yield sub.move.set_position(point_before).look_at( point_after).zero_roll_and_pitch().go(speed=SPEED) # go through the gate fprint('YOLO! Going through gate {}'.format(point_after)) yield sub.move.set_position(point_after).zero_roll_and_pitch().go( speed=SPEED) yield sub.vision_proxies.start_gate.stop() defer.returnValue(True)
def request_wrench_cb(self, msg): '''Callback for requesting a wrench''' time_now = rospy.Time.now() if (time_now - self.last_command_time) < self._min_command_time: return else: self.last_command_time = rospy.Time.now() force = rosmsg_to_numpy(msg.wrench.force) torque = rosmsg_to_numpy(msg.wrench.torque) wrench = np.hstack([force, torque]) success = False while not success: u, success = self.map(wrench) if not success: # If we fail to compute, shrink the wrench wrench = wrench * 0.75 continue thrust_cmds = [] # Assemble the list of thrust commands to send for name, thrust in zip(self.thruster_name_map, u): # > Can speed this up by avoiding appends if name in self.dropped_thrusters: thrust = 0 # Sending a command packet is an opportunity to detect thruster recovery # Simulate thruster deadband if np.fabs(thrust) < self.min_commandable_thrust: thrust = 0 thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust)) actual_wrench = self.B.dot(u) self.actual_wrench_pub.publish( msg_helpers.make_wrench_stamped(actual_wrench[:3], actual_wrench[3:], frame='/base_link')) mapper_wrench_error = wrench - actual_wrench self.wrench_error_pub.publish( msg_helpers.make_wrench_stamped(mapper_wrench_error[:3], mapper_wrench_error[3:], frame='/base_link')) self.thruster_pub.publish(thrust_cmds)
def run(sub): yield sub.vision_proxies.start_gate.start() # Add search pattern if needed... search_pattern = [] search = Searcher( sub, sub.vision_proxies.start_gate.get_pose, search_pattern) resp = None fprint('Searching...') resp = yield search.start_search(loop=False, timeout=100, spotings_req=1) if resp is None or not resp.found: fprint("No gate found...", msg_color="red") defer.returnValue(None) position = rosmsg_to_numpy(resp.pose.pose.position) orientation = rosmsg_to_numpy(resp.pose.pose.orientation) fprint('Gate\'s position in map is: {}'.format(position)) fprint('Gate\'s orientation in map is: {}'.format(orientation)) # Get the normal vector, which is assumed to be the [1,0,0] unit vector normal = tf.transformations.quaternion_matrix( orientation).dot(np.array([1, 0, 0, 0]))[0:3] fprint('Computed normal vector: {}'.format(normal)) # Computer points before and after the gate for the sub to go to point_before = position + FACTOR_DISTANCE_BEFORE * normal point_after = position - FACTOR_DISTANCE_AFTER * normal # go in front of gate fprint('Moving infront of gate {}'.format(point_before)) yield sub.move.set_position(point_before).look_at(point_after).zero_roll_and_pitch().go(speed=SPEED) # go through the gate fprint('YOLO! Going through gate {}'.format(point_after)) yield sub.move.set_position(point_after).zero_roll_and_pitch().go(speed=SPEED) yield sub.vision_proxies.start_gate.stop() defer.returnValue(True)
def run(sub): print_info = text_effects.FprintFactory(title=MISSION) print_bad = text_effects.FprintFactory(title=MISSION, msg_color="red") print_good = text_effects.FprintFactory(title=MISSION, msg_color="green") print_info.fprint("STARTING") # Wait for vision services, enable perception print_info.fprint("ACTIVATING PERCEPTION SERVICE") sub.vision_proxies.path_marker.start() pattern = [] #pattern = [sub.move.right(1), sub.move.forward(1), sub.move.left(1), sub.move.backward(1), # sub.move.right(2), sub.move.forward(2), sub.move.left(2), sub.move.backward(2)] s = Searcher(sub, sub.vision_proxies.path_marker.get_pose, pattern) resp = None print_info.fprint("RUNNING SEARCH PATTERN") resp = yield s.start_search(loop=False, timeout=TIMEOUT_SECONDS) if resp is None: print_bad.fprint("MARKER NOT FOUND") defer.returnValue(None) print_good.fprint("PATH MARKER POSE FOUND") print_info.fprint("TRANFORMING MARKER POSE TO /map FROM {}".format( resp.pose.header.frame_id)) cam_to_baselink = yield sub._tf_listener.get_transform( '/base_link', '/' + resp.pose.header.frame_id) cam_to_map = yield sub._tf_listener.get_transform( '/map', '/' + resp.pose.header.frame_id) marker_position = cam_to_map.transform_point( rosmsg_to_numpy(resp.pose.pose.position)) marker_orientation = cam_to_map.transform_quaternion( rosmsg_to_numpy(resp.pose.pose.orientation)) move = sub.move.set_orientation(marker_orientation).zero_roll_and_pitch() position = marker_position.copy() position[2] = move._pose.position[2] position[0] = position[0] + cam_to_baselink._p[0] position[1] = position[1] + cam_to_baselink._p[1] move = move.set_position(position) print_info.fprint("MOVING TO MARKER POSE") yield move.go(speed=0.2) print_good.fprint("ALIGNED TO PATH MARKER, DONE!") sub.vision_proxies.path_marker.stop() defer.returnValue(True)
def test_rosmsg_to_numpy_quaternion(self): '''Test a rosmsg conversion for a geometry_msgs/Quaternion''' # I know this is not a unit quaternion q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2) numpy_array = rosmsg_to_numpy(q) np.testing.assert_allclose( np.array([0.7, 0.7, 0.1, 0.2]), numpy_array )
def search(self): while True: info = 'FOUND: ' for buoy in self.buoys: res = yield self.sub.vision_proxies.buoy.get_pose(target=buoy) if res.found: self.buoys[buoy].update_position(rosmsg_to_numpy(res.pose.pose.position)) if self.buoys[buoy].position is not None: info += buoy + ' ' yield self.sub.nh.sleep(0.5) # Throttle service calls self.print_info(info)
def search(self): global markers markers = MarkerArray() pub_markers = yield self.nh.advertise('/torpedo/rays', MarkerArray) while True: info = 'CURRENT TARGETS: ' target = 'BCO' pub_markers.publish(markers) ''' In the event we want to attempt other targets beyond bare minimum for target in self.targets. Eventually we will want to take the best target, which is the TCX target. Priority targetting will come once we confirm we can actually unblock currently blocked targets. ''' print("REQUESTING POSE") res = yield self.vision_proxies.xyz_points.get_pose(target='board') if res.found: print("POSE FOUND!") self.ltime = res.pose.header.stamp self.targets[target].update_position( rosmsg_to_numpy(res.pose.pose.position)) self.normal = rosmsg_to_numpy(res.pose.pose.orientation)[:3] marker = Marker(ns='torp_board', action=visualization_msgs.Marker.ADD, type=Marker.ARROW, scale=Vector3(0.2, 0.5, 0), points=np.array( [Point(0, 0, 0), res.pose.pose.position])) marker.id = 3 marker.header.frame_id = '/base_link' marker.color.r = 1 marker.color.g = 0 marker.color.a = 1 markers.markers.append(marker) if self.targets[target].position is not None: print("TARGET IS NOT NONE") info += target + ' ' yield self.nh.sleep(0.1) # Throttle service calls self.print_info(info)
def run(sub): fprint('Getting Guess Locations') sub_start_position, sub_start_orientation = yield sub.tx_pose() gate_txros = yield sub.nh.get_service_client('/guess_location', GuessRequest) gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1')) gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2')) gate_1 = mil_ros_tools.rosmsg_to_numpy(gate_1_req.location.pose.position) gate_2 = mil_ros_tools.rosmsg_to_numpy(gate_2_req.location.pose.position) mid = (gate_1 + gate_2) / 2 yield sub.down(DOWN).set_orientation(sub_start_orientation).go( speed=DOWN_SPEED) yield sub.look_at_without_pitching(mid).go(speed=DOWN_SPEED) yield sub.set_position(mid).depth(DOWN).go(speed=SPEED)
def sanity_check(self, coordinate, timestamp): ''' Check if the observation is unreasonable. More can go here if we want. ''' if self.last_odom is None: return False linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear) if np.linalg.norm(linear_velocity) > self.max_velocity: return False return True
def sanity_check(self, coordinate, timestamp): ''' Check if the observation is unreasonable. More can go here if we want. ''' if self.last_odom is None: return False linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear) if np.linalg.norm(linear_velocity) > self.max_velocity: return False return True
def search(self): while True: info = 'FOUND: ' for buoy in self.buoys: res = yield self.sub.vision_proxies.buoy.get_pose(target=buoy) if res.found: self.buoys[buoy].update_position( rosmsg_to_numpy(res.pose.pose.position)) if self.buoys[buoy].position is not None: info += buoy + ' ' yield self.sub.nh.sleep(0.5) # Throttle service calls self.print_info(info)
def update_object(self, object_msg, class_probabilities): object_id = object_msg.id # Update the total class probabilities if object_id in self.object_map: self.object_map[object_id] += class_probabilities else: self.object_map[object_id] = class_probabilities total_probabilities = self.object_map[object_id] # Guess the type of object based most_likely_index = np.argmax(total_probabilities) most_likely_name = self.classifier.CLASSES[most_likely_index] # Unforuntely this doesn't really work' if most_likely_name in self.BLACK_OBJECT_CLASSES: object_scale = rosmsg_to_numpy(object_msg.scale) object_volume = object_scale.dot(object_scale) object_area = object_scale[:2].dot(object_scale[:2]) height = object_scale[2] if object_id in self.volume_means: self.volume_means[object_id].add_value(object_volume) self.area_means[object_id].add_value(object_area) else: self.volume_means[object_id] = RunningMean(object_volume) self.area_means[object_id] = RunningMean(object_area) running_mean_volume = self.volume_means[object_id].mean running_mean_area = self.area_means[object_id].mean if height > self.TOTEM_MIN_HEIGHT: black_guess = 'black_totem' else: black_guess_index = np.argmin( np.abs(self.BLACK_OBJECT_VOLUMES - running_mean_volume)) black_guess = self.POSSIBLE_BLACK_OBJECTS[black_guess_index] most_likely_name = black_guess rospy.loginfo( '{} current/running volume={}/{} area={}/{} height={}-> {}'. format(object_id, object_volume, running_mean_volume, object_area, running_mean_area, height, black_guess)) obj_title = object_msg.labeled_classification probability = class_probabilities[most_likely_index] rospy.loginfo('Object {} {} classified as {} ({}%)'.format( object_id, object_msg.labeled_classification, most_likely_name, probability * 100.)) if obj_title != most_likely_name: cmd = '{}={}'.format(object_id, most_likely_name) rospy.loginfo('Updating object {} to {}'.format( object_id, most_likely_name)) if not self.is_training: self.database_client(ObjectDBQueryRequest(cmd=cmd)) return most_likely_name
def request_wrench_cb(self, msg): '''Callback for requesting a wrench''' time_now = rospy.Time.now() if (time_now - self.last_command_time) < self._min_command_time: return else: self.last_command_time = rospy.Time.now() force = rosmsg_to_numpy(msg.wrench.force) torque = rosmsg_to_numpy(msg.wrench.torque) wrench = np.hstack([force, torque]) success = False while not success: u, success = self.map(wrench) if not success: # If we fail to compute, shrink the wrench wrench = wrench * 0.75 continue thrust_cmds = [] # Assemble the list of thrust commands to send for name, thrust in zip(self.thruster_name_map, u): # > Can speed this up by avoiding appends if name in self.dropped_thrusters: continue # Ignore dropped thrusters if np.fabs(thrust) < self.min_commandable_thrust: thrust = 0 thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust)) # TODO: Make this account for dropped thrusters actual_wrench = self.B.dot(u) self.actual_wrench_pub.publish( msg_helpers.make_wrench_stamped(actual_wrench[:3], actual_wrench[3:]) ) self.thruster_pub.publish(thrust_cmds)
def request_wrench_cb(self, msg): '''Callback for requesting a wrench''' time_now = rospy.Time.now() if (time_now - self.last_command_time) < self._min_command_time: return else: self.last_command_time = rospy.Time.now() force = rosmsg_to_numpy(msg.wrench.force) torque = rosmsg_to_numpy(msg.wrench.torque) wrench = np.hstack([force, torque]) success = False while not success: u, success = self.map(wrench) if not success: # If we fail to compute, shrink the wrench wrench = wrench * 0.75 continue thrust_cmds = [] # Assemble the list of thrust commands to send for name, thrust in zip(self.thruster_name_map, u): # > Can speed this up by avoiding appends if name in self.dropped_thrusters: continue # Ignore dropped thrusters # Simulate thruster deadband if np.fabs(thrust) < self.min_commandable_thrust: thrust = 0 thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust)) actual_wrench = self.B.dot(u) self.actual_wrench_pub.publish( msg_helpers.make_wrench_stamped(actual_wrench[:3], actual_wrench[3:]) ) self.thruster_pub.publish(thrust_cmds)
def set_pose_server(self, srv): '''Set the pose of the submarine TODO: Make this an 'abstract' method of Entity, and assign each new Entity a name/id ''' rospy.logwarn("Manually setting position of simulated vehicle") position = mil_ros_tools.rosmsg_to_numpy(srv.pose.position) self.body.setPosition(position) rotation_q = mil_ros_tools.rosmsg_to_numpy(srv.pose.orientation) rotation_norm = np.linalg.norm(rotation_q) velocity = mil_ros_tools.rosmsg_to_numpy(srv.twist.linear) angular = mil_ros_tools.rosmsg_to_numpy(srv.twist.angular) self.body.setLinearVel(velocity) self.body.setAngularVel(angular) # If the commanded rotation is valid if np.isclose(rotation_norm, 1., atol=1e-2): self.body.setQuaternion(mil_ros_tools.normalize(rotation_q)) else: rospy.logwarn("Commanded quaternion was not a unit quaternion, NOT setting rotation") return SimSetPoseResponse()
def check_continuity(self, odom): ''' On new odom message, find distance in position between this message and the last one. If the distance is > MAX_JUMP, raise the alarm ''' position = rosmsg_to_numpy(odom.pose.pose.position) if self.last_position is not None: jump = np.linalg.norm(position - self.last_position) if jump > self.MAX_JUMP and not self._killed: self._raised = True # Avoid raising multiple times rospy.logwarn('ODOM DISCONTINUITY DETECTED') self.ab.raise_alarm(problem_description='ODOM DISCONTINUITY DETECTED. JUMPED {} METERS'.format(jump), severity=5) self.last_position = position self.last_time = odom.header.stamp
def _sort_by_angle(objects, ray, start_point): """ _sort_by_angle: returns object list sorted by angle Parameters: objects: ray: directional unit vector start_point: base point for vector in map """ positions = [ mil_ros_tools.rosmsg_to_numpy(o.pose.position) for o in objects ] dots = [(p / np.linalg.norm(p) - start_point).dot(ray) for p in positions] idx = np.argsort(dots) return np.array(objects)[idx]
def _sort_by_angle(objects, ray, start_point): """ _sort_by_angle: returns object list sorted by angle Parameters: objects: ray: directional unit vector start_point: base point for vector in map """ positions = [ mil_ros_tools.rosmsg_to_numpy(o.pose.position) for o in objects ] dots = [(p / np.linalg.norm(p) - start_point).dot(ray) for p in positions] idx = np.argsort(dots) return np.array(objects)[idx]
def check_continuity(self, odom): ''' On new odom message, find distance in position between this message and the last one. If the distance is > MAX_JUMP, raise the alarm ''' position = rosmsg_to_numpy(odom.pose.pose.position) if self.last_position is not None: jump = np.linalg.norm(position - self.last_position) if jump > self.MAX_JUMP and not self._killed: self._raised = True # Avoid raising multiple times rospy.logwarn('ODOM DISCONTINUITY DETECTED') self.ab.raise_alarm( problem_description= 'ODOM DISCONTINUITY DETECTED. JUMPED {} METERS'.format( jump), severity=5) self.last_position = position self.last_time = odom.header.stamp
def _get_objects_within_cone(objects, start_point, ray, angle_tol, distance_tol): ray = ray / np.linalg.norm(ray) out = [] for o in objects: print '=' * 50 pos = mil_ros_tools.rosmsg_to_numpy(o.pose.position) print 'pos {}'.format(pos) dist = np.dot(pos - start_point, ray) print 'dist {}'.format(dist) if dist > distance_tol or dist < 0: continue vec_for_pos = pos - start_point vec_for_pos = vec_for_pos / np.linalg.norm(vec_for_pos) angle = np.arccos(vec_for_pos.dot(ray)) * 180 / np.pi print 'angle {}'.format(angle) if angle > angle_tol: continue out.append(o) return out
def _get_objects_within_cone(objects, start_point, ray, angle_tol, distance_tol): ray = ray / np.linalg.norm(ray) out = [] for o in objects: print '=' * 50 pos = mil_ros_tools.rosmsg_to_numpy(o.pose.position) print 'pos {}'.format(pos) dist = np.dot(pos - start_point, ray) print 'dist {}'.format(dist) if dist > distance_tol or dist < 0: continue vec_for_pos = pos - start_point vec_for_pos = vec_for_pos / np.linalg.norm(vec_for_pos) angle = np.arccos(vec_for_pos.dot(ray)) * 180 / np.pi print 'angle {}'.format(angle) if angle > angle_tol: continue out.append(o) return out
def search(self): while True: info = 'CURRENT TARGETS: ' target = 'BCO' ''' In the event we want to attempt other targets beyond bare minimum for target in self.targets. Eventually we will want to take the best target, which is the TCX target. Priority targetting will come once we confirm we can actually unblock currently blocked targets. ''' res = yield self.sub.vision_proxies.arm_torpedos.get_pose( target='board') if res.found: self.targets[target].update_position( rosmsg_to_numpy(res.pose.pose.position)) if self.targets[target].position is not None: info += target + ' ' yield self.sub.nh.sleep(0.5) # Throttle service calls self.print_info(info)
def search(self): global markers markers = MarkerArray() pub_markers = yield self.sub.nh.advertise('/roulette_wheel/rays', MarkerArray) while True: info = 'CURRENT TARGETS: ' target = 'Wheel' pub_markers.publish(markers) ''' In the event we want to attempt other targets beyond bare minimum for target in self.targets. Eventually we will want to take the best target, which is the TCX target. Priority targetting will come once we confirm we can actually unblock currently blocked targets. ''' res = yield self.sub.vision_proxies.arm_torpedos.get_pose( target='roulette_wheel') if res.found: self.ltime = res.pose.header.stamp self.targets[target].update_position( rosmsg_to_numpy(res.pose.pose.position)) marker = Marker( ns='roulette_wheel', action=visualization_msgs.Marker.ADD, type=Marker.ARROW, scale=Vector3(0.2, 0.5, 0), points=np.array([Point(0, 0, 0), res.pose.pose.position])) marker.id = 3 marker.header.frame_id = '/base_link' marker.color.r = 1 marker.color.g = 0 marker.color.a = 1 markers.markers.append(marker) if self.targets[target].position is not None: info += target + ' ' yield self.sub.nh.sleep(0.5) # Throttle service calls self.print_info(info)
def transform_to_baselink(sub, pinger_1_req, pinger_2_req): transform = yield sub._tf_listener.get_transform('/base_link', '/map') pinger_guess = [ transform._q_mat.dot( mil_ros_tools.rosmsg_to_numpy(x.location.pose.position) - transform._p) for x in (pinger_1_req, pinger_2_req) ] for idx, guess in enumerate(pinger_guess): marker = Marker( ns='pinger', action=visualization_msgs.Marker.ADD, type=Marker.ARROW, scale=Vector3(0.1, 0.2, 0), points=np.array( [Point(0, 0, 0), Point(guess[0], guess[1], guess[2])])) marker.id = idx marker.header.frame_id = '/base_link' marker.color.r = 0 marker.color.g = 1 marker.color.a = 1 global markers markers.markers.append(marker) defer.returnValue(pinger_guess)
def align_to_board(msg, sub): position = rosmsg_to_numpy(msg.position) yield sub.move.set_position(position).zero_roll_and_pitch().go()
def test_rosmsg_to_numpy_vector(self): '''Test a rosmsg conversion for a geometry_msgs/Vector''' v = Vector3(x=0.1, y=99., z=21.) numpy_array = rosmsg_to_numpy(v) np.testing.assert_allclose(np.array([0.1, 99., 21.]), numpy_array)
def from_Pose(cls, frame_id, msg): return cls(frame_id, rosmsg_to_numpy(msg.position), rosmsg_to_numpy(msg.orientation))
def align_to_board(msg, sub): position = rosmsg_to_numpy(msg.position) yield sub.move.set_position(position).zero_roll_and_pitch().go()
def run(sub): global SPEED global pub_cam_ray fprint('Enabling cam_ray publisher') pub_cam_ray = yield sub.nh.advertise('/dice/cam_ray', Marker) yield sub.nh.sleep(1) fprint('Connecting camera') cam_info_sub = yield sub.nh.subscribe('/camera/front/left/camera_info', CameraInfo) fprint('Obtaining cam info message') cam_info = yield cam_info_sub.get_next_message() model = PinholeCameraModel() model.fromCameraInfo(cam_info) enable_service = sub.nh.get_service_client("/dice/enable", SetBool) yield enable_service(SetBoolRequest(data=True)) dice_sub = yield sub.nh.subscribe('/dice/points', Point) found = {} history_tf = {} while len(found) != 2: fprint('Getting dice xy') dice_xy = yield dice_sub.get_next_message() found[dice_xy.z] = mil_ros_tools.rosmsg_to_numpy(dice_xy)[:2] fprint(found) out = yield get_transform(sub, model, found[dice_xy.z]) history_tf[dice_xy.z] = out if len(found) > 1: tmp = found.values()[0] - found.values()[1] # Make sure the two points are at least 100 pixels off diff = np.hypot(tmp[0], tmp[1]) fprint('Distance between buoys {}'.format(diff)) if diff < 40: found = {} yield enable_service(SetBoolRequest(data=False)) start = sub.move.zero_roll_and_pitch() yield start.go() for i in range(2): fprint('Hitting dice {}'.format(i)) # Get one of the dice dice, xy = found.popitem() fprint('dice {}'.format(dice)) ray, base = history_tf[dice] where = base + 3 * ray fprint(where) fprint('Moving!', msg_color='yellow') fprint('Current position: {}'.format(sub.pose.position)) fprint('zrp') yield sub.move.zero_roll_and_pitch().go(blind=True) yield sub.nh.sleep(4) fprint('hitting', msg_color='yellow') yield sub.move.look_at(where).go(blind=True, speed=SPEED) yield sub.nh.sleep(4) yield sub.move.set_position(where).go(blind=True, speed=SPEED) yield sub.nh.sleep(4) fprint('going back', msg_color='yellow') yield start.go(blind=True, speed=SPEED) yield sub.nh.sleep(4)
def run(self, args): self.vision_proxies.xyz_points.start() global SPEED global pub_cam_ray fprint('Enabling cam_ray publisher') pub_cam_ray = yield self.nh.advertise('/vamp/cam_ray', Marker) yield self.nh.sleep(1) fprint('Connecting camera') cam_info_sub = yield self.nh.subscribe( '/camera/front/left/camera_info', CameraInfo) fprint('Obtaining cam info message') cam_info = yield cam_info_sub.get_next_message() model = PinholeCameraModel() model.fromCameraInfo(cam_info) # enable_service = self.nh.get_service_client("/vamp/enable", SetBool) # yield enable_service(SetBoolRequest(data=True)) # try: # vamp_txros = yield self.nh.get_service_client('/guess_location', # GuessRequest) # vamp_req = yield vamp_txros(GuessRequestRequest(item='vampire_slayer')) # if vamp_req.found is False: # use_prediction = False # fprint( # 'Forgot to add vampires to guess?', # msg_color='yellow') # else: # fprint('Found vampires.', msg_color='green') # yield self.move.set_position(mil_ros_tools.rosmsg_to_numpy(vamp_req.location.pose.position)).depth(0.5).go(speed=0.4) # except Exception as e: # fprint(e) markers = MarkerArray() pub_markers = yield self.nh.advertise('/torpedo/rays', MarkerArray) pub_markers.publish(markers) ''' Move Pattern ''' yield self.move.left(1).go() yield self.nh.sleep(2) yield self.move.right(2).go() yield self.nh.sleep(2) yield self.move.down(.5).go() yield self.nh.sleep(2) yield self.move.up(.5).go() yield self.move.forward(.75).go() yield self.nh.sleep(2) yield self.move.right(.75).go() yield self.nh.sleep(2) yield self.move.backward(.75).go() yield self.move.left(1).go() ''' Did we find something? ''' res = yield self.vision_proxies.xyz_points.get_pose(target='buoy') MISSION = 'Vampires' print_info = FprintFactory(title=MISSION).fprint if res.found: print_info("CHARGING BUOY") target_pose = rosmsg_to_numpy(res.pose.pose.position) target_normal = rosmsg_to_numpy(res.pose.pose.orientation)[:2] print('Normal: ', target_normal) yield self.move.go(blind=True, speed=0.1) # Station hold transform = yield self._tf_listener.get_transform( '/map', '/base_link') target_position = target_pose # target_position = target_pose / target_normal sub_pos = yield self.tx_pose() print('Current Sub Position: ', sub_pos) print('Map Position: ', target_position) #sub_pos = transform._q_mat.dot(sub_pos[0] - transform._p) #target_position = target_position - sub_pos[0] # yield self.move.look_at_without_pitching(target_position).go(blind=True, speed=.25) #yield self.move.relative(np.array([0, target_position[1], 0])).go(blind=True, speed=.1) # Don't hit buoy yet print("MOVING TO X: ", target_position[0]) print("MOVING TO Y: ", target_position[1]) yield self.move.set_position( np.array([ target_position[0], target_position[1], target_position[2] ])).go(blind=True, speed=.1) # Go behind it #print('Going behind target') #yield self.move.right(4).go(speed=1) #yield self.move.forward(4).go(speed=1) #yield self.move.left(4).go(speed=1) # Hit buoy #print('Hitting Target') #yield self.move.strafe_backward(Y_OFFSET).go(speed=1) print_info("Slaying the Vampire, good job Inquisitor.") sub_pos = yield self.tx_pose() print('Current Sub Position: ', sub_pos) marker = Marker(ns='buoy', action=visualization_msgs.Marker.ADD, type=Marker.ARROW, scale=Vector3(0.2, 0.5, 0), points=np.array( [Point(0, 0, 0), res.pose.pose.position])) marker.id = 3 marker.header.frame_id = '/base_link' marker.color.r = 1 marker.color.g = 0 marker.color.a = 1 markers.markers.append(marker) pub_markers.publish(markers) yield self.nh.sleep(0.5) # Throttle service calls # print_info(info) self.vision_proxies.xyz_points.stop()
def run(sub): global SPEED global pub_cam_ray fprint('Enabling cam_ray publisher') pub_cam_ray = yield sub.nh.advertise('/dice/cam_ray', Marker) yield sub.nh.sleep(1) fprint('Connecting camera') cam_info_sub = yield sub.nh.subscribe('/camera/front/left/camera_info', CameraInfo) fprint('Obtaining cam info message') cam_info = yield cam_info_sub.get_next_message() model = PinholeCameraModel() model.fromCameraInfo(cam_info) enable_service = sub.nh.get_service_client("/dice/enable", SetBool) yield enable_service(SetBoolRequest(data=True)) dice_sub = yield sub.nh.subscribe('/dice/points', Point) found = {} history_tf = {} while len(found) != 2: fprint('Getting dice xy') dice_xy = yield dice_sub.get_next_message() found[dice_xy.z] = mil_ros_tools.rosmsg_to_numpy(dice_xy)[:2] fprint(found) out = yield get_transform(sub, model, found[dice_xy.z]) history_tf[dice_xy.z] = out if len(found) > 1: tmp = found.values()[0] - found.values()[1] # Make sure the two points are at least 100 pixels off diff = np.hypot(tmp[0], tmp[1]) fprint('Distance between buoys {}'.format(diff)) if diff < 40: found = {} yield enable_service(SetBoolRequest(data=False)) start = sub.move.zero_roll_and_pitch() yield start.go() for i in range(2): fprint('Hitting dice {}'.format(i)) # Get one of the dice dice, xy = found.popitem() fprint('dice {}'.format(dice)) ray, base = history_tf[dice] where = base + 3 * ray fprint(where) fprint('Moving!', msg_color='yellow') fprint('Current position: {}'.format(sub.pose.position)) fprint('zrp') yield sub.move.zero_roll_and_pitch().go(blind=True) yield sub.nh.sleep(4) fprint('hitting', msg_color='yellow') yield sub.move.look_at(where).go(blind=True, speed=SPEED) yield sub.nh.sleep(4) yield sub.move.set_position(where).go(blind=True, speed=SPEED) yield sub.nh.sleep(4) fprint('going back', msg_color='yellow') yield start.go(blind=True, speed=SPEED) yield sub.nh.sleep(4)
def run(self, args): global markers markers = MarkerArray() pub_markers = yield self.nh.advertise('/pinger/rays', MarkerArray) fprint('Getting Guess Locations') use_prediction = True try: pinger_txros = yield self.nh.get_service_client( '/guess_location', GuessRequest) pinger_1_req = yield pinger_txros( GuessRequestRequest(item='pinger_surface')) pinger_2_req = yield pinger_txros( GuessRequestRequest(item='pinger_shooter')) if pinger_1_req.found is False or pinger_2_req.found is False: use_prediction = False fprint('Forgot to add pinger to guess server?', msg_color='yellow') else: fprint('Found two pinger guesses', msg_color='green') pinger_guess = yield self.transform_to_baselink( self, pinger_1_req, pinger_2_req) fprint(pinger_guess) except Exception as e: fprint('Failed to /guess_location. Procceding without guess', msg_color='yellow') use_prediction = False fprint(e) while True: fprint('=' * 50) fprint("waiting for message") p_message = yield self.pinger_sub.get_next_message() pub_markers.publish(markers) fprint(p_message) # Ignore freuqnecy if not abs(p_message.freq - FREQUENCY) < FREQUENCY_TOL: fprint("Ignored! Recieved Frequency {}".format(p_message.freq), msg_color='red') if use_prediction: fprint("Moving to random guess") yield self.go_to_random_guess(self, pinger_1_req, pinger_2_req) continue # Ignore magnitude from processed ping p_position = mil_ros_tools.rosmsg_to_numpy(p_message.position) vec = p_position / np.linalg.norm(p_position) if np.isnan(vec).any(): fprint('Ignored! nan', msg_color='red') if use_prediction: yield self.go_to_random_guess(self, pinger_1_req, pinger_2_req) continue # Tranform hydrophones vector to relative coordinate transform = yield self._tf_listener.get_transform( '/base_link', '/hydrophones') vec = transform._q_mat.dot(vec) fprint('Transformed vec: {}'.format(vec)) marker = Marker(ns='pinger', action=visualization_msgs.Marker.ADD, type=Marker.ARROW, scale=Vector3(0.2, 0.5, 0), points=np.array([ Point(0, 0, 0), Point(vec[0], vec[1], vec[2]) ])) marker.id = 3 marker.header.frame_id = '/base_link' marker.color.r = 1 marker.color.g = 0 marker.color.a = 1 markers.markers.append(marker) # Check if we are on top of pinger if abs(vec[0]) < POSITION_TOL and abs( vec[1]) < POSITION_TOL and vec[2] < Z_POSITION_TOL: if not use_prediction: fprint("Arrived to pinger!") break sub_position, _ = yield self.tx_pose() dists = [ np.linalg.norm(sub_position - mil_ros_tools.rosmsg_to_numpy( x.location.pose.position)) for x in (pinger_1_req, pinger_2_req) ] pinger_id = np.argmin(dists) # pinger_id 0 = pinger_surface # pinger_id 1 = pinger_shooter yield self.nh.set_param("pinger_where", int(pinger_id)) break vec[2] = 0 if use_prediction: pinger_guess = yield self.transform_to_baselink( self, pinger_1_req, pinger_2_req) fprint('Transformed guess: {}'.format(pinger_guess)) # Check if the pinger aligns with guess # check, vec = self.check_with_guess(vec, pinger_guess) fprint('move to {}'.format(vec)) yield self.fancy_move(self, vec) fprint('Arrived to hydrophones! Going down!') yield self.move.to_height(PINGER_HEIGHT).zero_roll_and_pitch().go( speed=0.1)
def run(sub): yield sub.move.downward(1).go() fprint('Begin search for gates') # Search 4 quadrants deperated by 90 degrees for the gate start = sub.move.zero_roll_and_pitch() # Pitch up and down to populate pointcloud so = SonarObjects(sub, [start.pitch_down_deg(7), start] * 10) transform = yield sub._tf_listener.get_transform('/map', '/base_link') # [1, 0, 0] is front vector for sub ray = transform._q_mat.dot(np.array([1, 0, 0])) # Start scan and search res = yield so.start_search_in_cone(transform._p, ray, angle_tol=60, distance_tol=11, speed=0.1, clear=True, c_func=find_gate) del so fprint('Found {} objects'.format(len(res.objects))) # Didn't find enough objects if len(res.objects) < 2: fprint('No objects') # Search for two objects that satisfy the gate gate_points = find_gate(res.objects, ray) if gate_points is None: fprint('No valid gates') if gate_points is None: fprint('Returning') yield start.go() defer.returnValue(False) print() # Find midpoint between the two poles/objects mid_point = gate_points[0] + gate_points[1] mid_point = mid_point / 2 # Offset z so we don't hit the bar mid_point[2] = mid_point[2] - 0.5 fprint('Midpoint: {}'.format(mid_point)) yield sub.move.look_at(mid_point).go(speed=SPEED) fprint('Moving!', msg_color='yellow') yield sub.move.set_position(mid_point).go(speed=SPEED) normal = mid_point - sub.pose.position normal[2] = 0 normal = normal / np.linalg.norm(normal) fprint('Normal {} '.format(normal)) fprint('Moving past the gate', msg_color='yellow') yield sub.move.set_position(mid_point + DIST_AFTER_GATE * normal).go(speed=SPEED) fprint('Checking if already seen qualification pole') current_position = sub.pose.position objects = SonarObjects._get_objects_within_cone(res.objects, current_position, normal, angle_tol=100, distance_tol=11) objects = None print(objects) if objects is None or len(objects) < 1: fprint('Searching for qualifiction pole') start = sub.move.zero_roll_and_pitch() so = SonarObjects(sub, [start.pitch_down_deg(7), start] * 4) so_objects = yield so.start_search_in_cone(sub.pose.position, normal, angle_tol=50, distance_tol=11, speed=0.1, clear=True) objects = so_objects.objects if objects is None: fprint('Failed to find qualification pole') defer.retuenValue(False) fprint('Found pole') objects = SonarObjects._sort_by_angle(objects, normal, current_position) pole = rosmsg_to_numpy(objects[0].pose.position) fprint(pole) pole[2] = sub.pose.position[2] normal_pole = pole - sub.pose.position normal_pole = normal_pole / np.linalg.norm(normal_pole) move1 = pole move1 = move1 - normal_pole * 2 fprint(move1) yield sub.move.set_position(move1).go(speed=SPEED_CAREFUL) yield sub.move.left(1.7).go(speed=SPEED_CAREFUL) yield sub.move.forward(1.7).go(speed=SPEED_CAREFUL) yield sub.move.right(3.4).go(speed=SPEED_CAREFUL) yield sub.move.backward(1.7).go(speed=SPEED_CAREFUL) yield sub.move.left(1.7).go(speed=SPEED_CAREFUL) yield sub.move.backward(0.5).go(speed=SPEED_CAREFUL) yield sub.move.look_at(mid_point).go(speed=SPEED_CAREFUL) fprint('Going to front of gate') yield sub.move.set_position(mid_point + DIST_AFTER_GATE * normal).go(speed=SPEED) yield sub.move.set_position(mid_point).go(speed=SPEED_CAREFUL) yield sub.move.set_position(mid_point - DIST_AFTER_GATE * normal).go(speed=SPEED)
def run(sub): global markers markers = MarkerArray() pub_markers = yield sub.nh.advertise('/pinger/rays', MarkerArray) fprint('Getting Guess Locations') pinger_txros = yield sub.nh.get_service_client('/guess_location', GuessRequest) pinger_1_req = yield pinger_txros(GuessRequestRequest(item='pinger1')) pinger_2_req = yield pinger_txros(GuessRequestRequest(item='pinger2')) use_prediction = True if pinger_1_req.found is False or pinger_2_req.found is False: use_prediction = False fprint('Forgot to add pinger to guess server?', msg_color='yellow') else: fprint('Found two pinger guesses', msg_color='green') pinger_guess = yield transform_to_baselink(sub, pinger_1_req, pinger_2_req) fprint(pinger_guess) hydro_processed = yield sub.nh.subscribe('/hydrophones/processed', ProcessedPing) while True: fprint('=' * 50) pub_markers.publish(markers) p_message = yield hydro_processed.get_next_message() # Ignore freuqnecy if not abs(p_message.freq - FREQUENCY) < FREQUENCY_TOL: fprint( "Ignored! Recieved Frequency {}".format(p_message.freq), msg_color='red') if use_prediction: yield go_to_random_guess(sub, pinger_1_req, pinger_2_req) continue # Ignore magnitude from processed ping p_position = mil_ros_tools.rosmsg_to_numpy(p_message.position) vec = p_position / np.linalg.norm(p_position) if np.isnan(vec).any(): fprint('Ignored! nan', msg_color='red') if use_prediction: yield go_to_random_guess(sub, pinger_1_req, pinger_2_req) continue # Tranform hydrophones vector to relative coordinate transform = yield sub._tf_listener.get_transform( '/base_link', '/hydrophones') vec = transform._q_mat.dot(vec) fprint('Transformed vec: {}'.format(vec)) marker = Marker( ns='pinger', action=visualization_msgs.Marker.ADD, type=Marker.ARROW, scale=Vector3(0.2, 0.5, 0), points=np.array([Point(0, 0, 0), Point(vec[0], vec[1], vec[2])])) marker.id = 3 marker.header.frame_id = '/base_link' marker.color.r = 1 marker.color.g = 0 marker.color.a = 1 markers.markers.append(marker) # Check if we are on top of pinger if abs(vec[0]) < POSITION_TOL and abs(vec[1] < POSITION_TOL): sub_position, _ = yield sub.tx_pose() dists = [ np.linalg.norm(sub_position - mil_ros_tools.rosmsg_to_numpy( x.location.pose.position)) for x in (pinger_1_req, pinger_2_req) ] pinger_id = np.argmin(dists) yield sub.nh.set_param("pinger_where", int(pinger_id)) break vec[2] = 0 if use_prediction: pinger_guess = yield transform_to_baselink(sub, pinger_1_req, pinger_2_req) fprint('Transformed guess: {}'.format(pinger_guess)) # Check if the pinger aligns with guess check, vec = check_with_guess(vec, pinger_guess) fprint('move to {}'.format(vec)) yield fancy_move(sub, vec) fprint('Arrived to hydrophones! Going down!') yield sub.move.to_height(PINGER_HEIGHT).zero_roll_and_pitch().go(speed=0.1)
def from_Pose(cls, frame_id, msg): return cls(frame_id, rosmsg_to_numpy(msg.position), rosmsg_to_numpy(msg.orientation))
class BallDrop(SubjuGator): @util.cancellableInlineCallbacks def run(self, args): try: ree = rospy.ServiceProxy('/vision/garlic/enable', SetBool) resp = ree(True) if not resp: print("Error, failed to init neural net.") return except rospy.ServiceException, e: print("Service Call Failed") fprint('Enabling cam_ray publisher') yield self.nh.sleep(1) fprint('Connecting camera') cam_info_sub = yield self.nh.subscribe('/camera/down/camera_info', CameraInfo) fprint('Obtaining cam info message') cam_info = yield cam_info_sub.get_next_message() cam_center = np.array([cam_info.width / 2, cam_info.height / 2]) cam_norm = np.sqrt(cam_center[0]**2 + cam_center[1]**2) fprint('Cam center: {}'.format(cam_center)) model = PinholeCameraModel() model.fromCameraInfo(cam_info) # enable_service = self.nh.get_service_client("/vamp/enable", SetBool) # yield enable_service(SetBoolRequest(data=True)) try: vamp_txros = yield self.nh.get_service_client( '/guess_location', GuessRequest) ball_drop_req = yield vamp_txros( GuessRequestRequest(item='ball_drop')) if ball_drop_req.found is False: use_prediction = False fprint('Forgot to add ball_drop to guess?', msg_color='yellow') else: fprint('Found ball_drop.', msg_color='green') yield self.move.set_position( mil_ros_tools.rosmsg_to_numpy( ball_drop_req.location.pose.position)).depth( TRAVEL_DEPTH).go(speed=FAST_SPEED) except Exception as e: fprint(str(e) + 'Forgot to run guess server?', msg_color='yellow') ball_drop_sub = yield self.nh.subscribe('/bbox_pub', Point) yield self.move.to_height(SEARCH_HEIGHT).zero_roll_and_pitch().go( speed=SPEED) while True: fprint('Getting location of ball drop...') ball_drop_msg = yield ball_drop_sub.get_next_message() ball_drop_xy = mil_ros_tools.rosmsg_to_numpy(ball_drop_msg)[:2] vec = ball_drop_xy - cam_center fprint("Vec: {}".format(vec)) vec = vec / cam_norm vec[1] = -vec[1] fprint("Rel move vec {}".format(vec)) if np.allclose(vec, np.asarray(0), atol=50): break vec = np.append(vec, 0) yield self.move.relative_depth(vec).go(speed=SPEED) fprint('Centered, going to depth {}'.format(HEIGHT_BALL_DROPER)) yield self.move.to_height(HEIGHT_BALL_DROPER).zero_roll_and_pitch().go( speed=SPEED) fprint('Dropping marker') yield self.actuators.drop_marker()
def test_rosmsg_to_numpy_custom(self): '''Test a rosmsg conversion for a custom msg''' pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14) numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta']) np.testing.assert_allclose(np.array([1.0, 2.0, 3.14]), numpy_array)