def __init__(self, base_topic, debugging_on):
self.base_topic = base_topic
self.debug = Debugger(debugging_on, debugging_on)
self.odom_array = OdometryArray()
self.odom_lock = threading.Lock()
self.bridge = CvBridge()
self.daov = rospy.get_param("~roomba_estimator_settings/%s_aov" %
base_topic)
self.tf_buffer = tf2_ros.Buffer()
tf2_ros.TransformListener(self.tf_buffer)
# Make sure at least one transform exists before starting
self.tf_buffer.lookup_transform('map',
'bottom_camera_rgb_optical_frame',
rospy.Time(0), rospy.Duration(3.0))
rospy.Subscriber("/{}/rgb/image_raw".format(base_topic), Image,
self.callback)
rospy.Subscriber("/roombas", OdometryArray, self.roombas_callback)
self.publisher = rospy.Publisher("/roombas",
OdometryArray,
queue_size=10)
def _publish(self, time):
SCORE_PUBLISH_THRESHOLD = 0.99
out_msg = OdometryArray()
for f in self._filters:
if f['score'] < SCORE_PUBLISH_THRESHOLD:
continue
odom = f['filter'].get_state(time)
self._debug_pub.publish(odom)
out_msg.data.append(odom)
self._pub.publish(out_msg)
def _publish_empty(self):
out_msg = OdometryArray()
self._pub.publish(out_msg)
def _publish(self, time):
out_msg = OdometryArray()
odom = self._filter.get_state(time)
out_msg.data.append(odom)
self._pub.publish(out_msg)
self._debug_pub.publish(odom)
import tf2_ros as tf2
from geometry_msgs.msg import TransformStamped
from iarc7_msgs.msg import OdometryArray
from nav_msgs.msg import Odometry
if __name__ == '__main__':
rospy.init_node('stupid_roomba')
tf2_broadcaster = tf2.TransformBroadcaster()
pub = rospy.Publisher('/roombas', OdometryArray, queue_size=10)
vx = 0.33
roomba_msg = OdometryArray()
roomba = Odometry()
roomba.header.frame_id = 'map'
roomba.child_frame_id = 'roomba0/base_link'
roomba.pose.pose.orientation.w = 1.0
roomba.twist.twist.linear.x = vx
roomba_msg.data.append(roomba)
tf_msg = TransformStamped()
tf_msg.header.frame_id = 'map'
tf_msg.child_frame_id = 'roomba0/base_link'
tf_msg.transform.rotation.w = 1.0
start_time = rospy.Time.now()
rate = rospy.Rate(30)
def callback(_):
msg = OdometryArray()
roomba_pub.publish(msg)
def roomba_odom_callback(msg, topic, data={}):
if not 'cur_odoms' in data:
data['cur_odoms'] = {}
data['cur_odoms'][topic] = msg
if 'last_time' in data:
earliest_publish_time = (data['last_time']
+ rospy.Duration(1.0/max_roomba_output_freq))
if earliest_publish_time > rospy.Time.now():
return
data['last_time'] = rospy.Time.now()
odom_msg = OdometryArray()
odom_msg.data = data['cur_odoms'].values()
state_msg = RoombaStateStampedArray()
for odom in odom_msg.data:
roomba_msg = RoombaStateStamped()
roomba_msg.odom = odom
roomba_msg.roomba_id = odom.child_frame_id
roomba_id = odom.child_frame_id.split('/')[0]
if roomba_id in roomba_states:
state_data = roomba_states[roomba_id]
roomba_msg.turning = state_data.data
roomba_msg.moving_forward = not state_data.data
state_msg.roombas.append(roomba_msg)
if publish_ground_truth_roombas:
roomba_pub.publish(odom_msg)
roomba_publisher.publish(state_msg)
if publish_noisy_roombas and last_drone_position is not None:
observations = []
for roomba_odom in data['cur_odoms'].values():
roomba_msg = RoombaDetection()
roomba_pos = np.array((roomba_odom.pose.pose.position.x,
roomba_odom.pose.pose.position.y,
roomba_odom.pose.pose.position.z))
drone_pos = np.array((last_drone_position.x,
last_drone_position.y,
last_drone_position.z))
dist = np.linalg.norm(roomba_pos - drone_pos)
p = 1 / (1 + dist / noisy_roombas_d0)
observed = np.random.random() <= p
if observed:
observed_pos = np.random.normal(
roomba_pos[:-1],
(dist * noisy_roombas_uncertainty_scale,
dist * noisy_roombas_uncertainty_scale))
roomba_msg.pose.x = observed_pos[0]
roomba_msg.pose.y = observed_pos[1]
observed_theta = np.random.normal(
np.arctan2(roomba_odom.twist.twist.linear.y,
roomba_odom.twist.twist.linear.x),
1)
roomba_msg.pose.theta = observed_theta % (2*np.pi)
c = dist * noisy_roombas_uncertainty_scale
observations.append(roomba_msg)
out_msg = RoombaDetectionFrame()
out_msg.header = msg.header
for point in ((-1, 1),
(-1, -1),
(1, -1),
(1, 1)):
new_point = Point32()
new_point.x = point[0] * 10
new_point.y = point[1] * 10
out_msg.detection_region.points.append(new_point)
out_msg.roombas = observations
roomba_noisy_pub.publish(out_msg)