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 _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 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 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)
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)