def check_contact(self, msg): if not self.launched: return if len(msg.states) == 0: # If there is no impact don't worry about it. return torpedo_name = "torpedo::body::bodycol" board_name = "torpedo_board::hole_1::hole_1_col" real_state = None for state in msg.states: if state.collision1_name == torpedo_name: real_state = state other_collison_name = state.collision2_name break if state.collision2_name == torpedo_name: real_state = state other_collison_name = state.collision1_name break if real_state is None: return # Generally, if the torpedo is laying on the ground the collision force will be very small. # So if the force is really small, we assume the impact collision has occured before the torpedo was launched. print np.abs( np.linalg.norm( msg_helpers.rosmsg_to_numpy(real_state.total_wrench.force))) if np.abs( np.linalg.norm( msg_helpers.rosmsg_to_numpy( real_state.total_wrench.force))) < 10: rospy.loginfo( "Torpedo probably still on the ground, still waiting.") return # Now the torpedo has impacted something, publish what it hit. rospy.loginfo('Impact detected!') self.launched = False rospy.loginfo(other_collison_name) self.contact_pub.publish(other_collison_name)
def _cb(self, msg): ''' Internal callback on new points_of_interest updates ''' self.last_msg = msg for poi in self.last_msg.pois: if poi.name not in self.defers: continue position = rosmsg_to_numpy(poi.position) defers = self.defers.pop(poi.name) while len(defers): defers.pop().callback(position)
def from_polygon(cls, polygon): ''' Creates a RectFinder from a geometry_msgs/Polygon message. The length of the Rect becomes the range of x/y (whichever larger) The width of the Rect becomes the range of x/y (whichever shorter) ''' assert isinstance(polygon, Polygon) arr = rosmsg_to_numpy(polygon.points) # If it contains just one point, treat the x/y and length/width if arr.shape[0] == 1: return cls(arr[:, 0][0], arr[:, 1][0]) else: return cls(np.ptp(arr[:, 0]), np.ptp(arr[:, 1]))
def check_contact(self, msg): if not self.launched: return if len(msg.states) == 0: # If there is no impact don't worry about it. return torpedo_name = "torpedo::body::bodycol" real_state = None for state in msg.states: if state.collision1_name == torpedo_name: real_state = state other_collison_name = state.collision2_name break if state.collision2_name == torpedo_name: real_state = state other_collison_name = state.collision1_name break if real_state is None: return # Generally, if the torpedo is laying on the ground the collision force will be very small. # So if the force is really small, we assume the impact collision has occured before the torpedo was launched. print np.abs(np.linalg.norm(msg_helpers.rosmsg_to_numpy(real_state.total_wrench.force))) if np.abs(np.linalg.norm(msg_helpers.rosmsg_to_numpy(real_state.total_wrench.force))) < 10: rospy.loginfo("Torpedo probably still on the ground, still waiting.") return # Now the torpedo has impacted something, publish what it hit. rospy.loginfo('Impact detected!') self.launched = False rospy.loginfo(other_collison_name) self.contact_pub.publish(other_collison_name)
def get(self, name, only_fresh=False): ''' Get the position of POI in the global frame as a 3x1 numpy array. Note: returns a defered object which will be calledback when the POI is found, which may be immediately @param name: the name of the POI @param only_fresh: if the POI is already known, wait for a fresh message before returning ''' if self.last_msg is not None and not only_fresh: for poi in self.last_msg.pois: if poi.name == name: yield defer.returnValue(rosmsg_to_numpy(poi.position)) res = defer.Deferred() if name in self.defers: self.defers[name].append(res) else: self.defers[name] = [res] poi = yield res defer.returnValue(poi)