def sensor_callback(self,sens):
# Publish state estimate
self.estimate(sens)
est_msg = RobotPose()
est_msg.header.stamp = sens.header.stamp
est_msg.pose.x = self.x[0]
est_msg.pose.y = self.x[1]
est_msg.pose.theta = self.x[2]
self.pub_est.publish(est_msg)
def step(self, event):
self.x = self.x + self.step_size * self.vt * math.cos(self.theta)
self.y = self.y + self.step_size * self.vt * math.sin(self.theta)
self.theta = self.theta + self.step_size * self.vrot
if self.start_flag:
self.x += numpy.random.normal(0.0, self.model_noise_trans)
self.y += numpy.random.normal(0.0, self.model_noise_trans)
self.theta += numpy.random.normal(0.0, self.model_noise_rot)
time = rospy.Time.now()
pose_msg = RobotPose()
pose_msg.header.stamp = time
pose_msg.pose.x = self.x
pose_msg.pose.y = self.y
pose_msg.pose.theta = self.theta
self.pub_pose.publish(pose_msg)
sensor_msg = self.get_sensor_data()
sensor_msg.header.stamp = time
self.pub_sens.publish(sensor_msg)