def init_state(self):
if self.model == "simple_nonlinear":
self.model = SimpleNonlinearModel()
elif self.model == "linear":
self.model = LinearModel()
else:
rospy.logerror("Model type '%s' unknown" % self.model)
raise Exception("Model type '%s' unknown" % self.model)
class SimAdapter(object):
def __init__(self):
pass
def start(self):
rospy.init_node('sim_adapter')
self.init_params()
self.init_state()
self.init_vars()
self.init_publishers()
self.init_subscribers()
self.init_timers()
rospy.spin()
def init_vars(self):
self.latest_cmd_msg = control_mode_output()
self.motor_enable = False
self.thrust_cmd = 0.0
def init_state(self):
if self.model == "simple_nonlinear":
self.model = SimpleNonlinearModel()
elif self.model == "linear":
self.model = LinearModel()
else:
rospy.logerror("Model type '%s' unknown" % self.model)
raise Exception("Model type '%s' unknown" % self.model)
def init_params(self):
self.model = rospy.get_param("~model", "simple_nonlinear")
def init_publishers(self):
# Publishers
self.pub_odom = rospy.Publisher('odom', Odometry)
def init_subscribers(self):
# Subscribers
self.control_input_sub = rospy.Subscriber('controller_mux/output', control_mode_output, self.control_input_callback)
self.motor_enable_sub = rospy.Subscriber('teleop_flyer/motor_enable', Bool, self.motor_enable_callback)
def init_timers(self):
self.simulation_timer = Timer(rospy.Duration(1/50.0), self.simulation_timer_callback)
# Subscriber callbacks:
def control_input_callback(self, msg):
rospy.logdebug('Current command is: ' + str(msg))
self.latest_cmd_msg = msg
def motor_enable_callback(self, msg):
if msg.data != self.motor_enable:
#rospy.loginfo('Motor enable: ' + str(msg.data))
self.motor_enable = msg.data
# Timer callbacks:
def simulation_timer_callback(self, event):
if False:
print 'last_expected: ', event.last_expected
print 'last_real: ', event.last_real
print 'current_expected: ', event.current_expected
print 'current_real: ', event.current_real
print 'current_error: ', (event.current_real - event.current_expected).to_sec()
print 'profile.last_duration:', event.last_duration.to_sec()
if event.last_real:
print 'last_error: ', (event.last_real - event.last_expected).to_sec(), 'secs'
print
if event.last_real is None:
dt = 0.0
else:
dt = (event.current_real - event.last_real).to_sec()
self.update_controller(dt)
self.update_state(dt)
#rospy.loginfo("position: " + str(self.position) + " velocity: " + str(self.velocity) + " thrust_cmd: " + str(self.thrust_cmd))
self.publish_odometry()
def update_state(self, dt):
# The following model is completely arbitrary and should not be taken to be representative of
# real vehicle performance!
# But, it should be good enough to test out control modes etc.
self.model.update(dt)
def update_controller(self, dt):
lcm = self.latest_cmd_msg
self.model.set_input(np.array([lcm.yaw_cmd, lcm.pitch_cmd, lcm.roll_cmd, lcm.alt_cmd, lcm.motors_on]), dt)
#rospy.loginfo("thrust_cmd = %f, dt = %f" % (self.thrust_cmd, dt))
def publish_odometry(self):
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "/ned"
oppp = odom_msg.pose.pose.position
oppp.x, oppp.y, oppp.z = self.model.get_position()
ottl = odom_msg.twist.twist.linear
ottl.x, ottl.y, ottl.z = self.model.get_velocity()
oppo = odom_msg.pose.pose.orientation
oppo.x, oppo.y, oppo.z, oppo.w = self.model.get_orientation()
otta = odom_msg.twist.twist.angular
otta.x, otta.y, otta.z = self.model.get_angular_velocity()
self.pub_odom.publish(odom_msg)
