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)