def __init__(self):
p3_rpu.PeriodicNode.__init__(self, 'ros_pat_publish_transform')
pos_ned = [0, 0, -1]
self.T_w2b = np.eye(4)
dm_ae = p1_fw_dyn.DynamicModel()
Xe, Ue = dm_ae.trim({
'h': 0,
'va': 8,
'gamma': 0
},
report=True,
debug=True)
va, alpha, beta = Xe[p3_fr.SixDOFAeroEuler.sv_slice_vaero]
#va, alpha, beta = 10, np.deg2rad(3), np.deg2rad(0)
phi, theta, psi = Xe[p3_fr.SixDOFAeroEuler.sv_slice_eul]
#phi, theta, psi = np.deg2rad(0), np.deg2rad(20), np.deg2rad(0)
p3_u.set_rot(self.T_w2b, p3_al.rmat_of_euler([phi, theta, psi]))
self.T_b2w = np.linalg.inv(self.T_w2b)
p3_u.set_trans(self.T_b2w, pos_ned)
self.T_a2b = np.eye(4)
p3_u.set_rot(self.T_a2b, p3_fr.R_aero_to_body(alpha, beta))
self.T_b2a = self.T_a2b #np.linalg.inv(self.T_a2b) # FIXME
self.tf_pub = p3_rpu.TransformPublisher()
self.marker_pub = p3_rpu.PoseArrayPublisher(dae='glider.dae')
def __init__(self, traj, time_factor=1.):
self.time_factor = time_factor
self.traj = traj
self.tf_pub = pru.TransformPublisher()
#self.pose_pub = pru.PoseArrayPublisher(dae='quad.dae')
self.pose_pub = pru.PoseArrayPublisher(dae='glider.dae')
self.traj_pub = pru.TrajectoryPublisher(self.traj)
self.fdm = fdm.FDM()
self.df = ctl.DiffFlatness()
def __init__(self):
self.h_ctl = HoverController()
self.wps_ctl = WPSeqCtl(self.h_ctl)
cc_rpu.PeriodicNode.__init__(self, 'test_sjtu_drone')
self.twist_pub = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
self.transform_pub = p3_rpu.TransformPublisher()
self.marker_pub = p3_rpu.PoseArrayPublisher(dae='quad_murat.dae')
self.odom_pub = p3_rpu.OdomPublisher()
self.pose_lst = cc_rpu.PoseListener('/drone/gt_pose')
def __init__(self):
_fdm = fdm.FDM()
_ctl = ctl.ZAttController(_fdm)
self.sim = pmu.Sim(_fdm, _ctl)
self.tf_pub = pru.TransformPublisher()
self.pose_pub = pru.PoseArrayPublisher()
rospy.Subscriber('/joy',
sensor_msgs.msg.Joy,
self.on_joy_msg,
queue_size=1)
#self.input = ctl.StepZInput(0.1)
#self.input = ctl.StepEulerInput(pal.e_phi, _a=np.deg2rad(1.), p=4, dt=1)
self.input = ctl.StepEulerInput(pal.e_theta,
_a=np.deg2rad(1.),
p=4,
dt=1)
def __init__(self, traj, time_factor=1.):
traj = p3_fw_guid.CircleRefTraj2(c=[0, 0, 10], r=15)
self.time_factor = time_factor
self.traj = traj
self.tf_pub = pru.TransformPublisher()
self.pose_pub = pru.PoseArrayPublisher(dae='glider.dae')
self.traj_pub = pru.TrajectoryPublisher(self.traj)
self.carrot_pub = pru.MarkerPublisher('guidance/goal', 'w_ned')
self.carrot_pub2 = pru.MarkerPublisher('guidance/goal2', 'b_frd', argb=(1., 1., 1., 0.))
self.fdm = fdm.FDM()
param_filename='/home/poine/work/pat/data/vehicles/cularis.xml'
self.dm = p1_fw_dyn.DynamicModel(param_filename)
self.guidance = p3_fw_guid.Guidance(self.dm, traj, {'h':0, 'va':15, 'gamma':0})
self.df = ctl.DiffFlatness()
def __init__(self, _traj=None, time_factor=1.):
traj = p3_traj3d.CircleRefTraj(c=[0, 0, -20], r=15)
#traj = p3_fw_guid.LineRefTraj(p1=[0, 0, 0], p2=[50, 50, 0])
#traj = p3_fw_guid.SquareRefTraj()
#traj = p3_fw_guid.OctogonRefTraj()
self.time_factor = time_factor
param_filename = '/home/poine/work/pat/data/vehicles/cularis.xml'
_fdm = p1_fw_dyn.DynamicModel(param_filename)
_fms = p3_fms.FMS(_fdm, {'h': 0, 'va': 10, 'gamma': 0})
#_ctl = p3_fw_guid.Guidance(_fdm, traj, {'h':0, 'va':10, 'gamma':0})
#_ctl = p3_guid.GuidanceThermal(_fdm, traj, {'h':0, 'va':10, 'gamma':0})
#_atm = p3_atm.AtmosphereCstWind([0, 0, -1])
#_atm = p3_atm.AtmosphereThermal1()
_atm = p3_atm.AtmosphereThermalMulti()
#_atm = p3_atm.AtmosphereWharington()#center=None, radius=50, strength=-2, cst=[0, 0, 0])
#_atm = p3_atm.AtmosphereGedeon(center=[55, 0, 0], radius=50, strength=-2, cst=[0, 0, 0])
#_atm = p3_atm.AtmosphereRidge()
self.sim = pmu.Sim(_fdm, _fms, _atm)
self.tf_pub = p3_rpu.TransformPublisher()
self.carrot_pub = p3_rpu.MarkerPublisher('guidance/goal',
'w_ned',
scale=(0.25, 0.25, 0.5))
self.traj_pub = p3_rpu.TrajectoryPublisher2(traj, ms=0.2)
#self.pose_pub = p3_rpu.PoseArrayPublisher(dae='glider.dae')
self.pose_pub = p3_rpu.PoseArrayPublisher(dae='ds_glider_full.dae')
self.atm_pub = None #p3_rpu.AtmPointCloudPublisher(_atm)
self.status_pub = p3_rpu.GuidanceStatusPublisher()
self.atm_disp_dyn_rec_client = dynamic_reconfigure.client.Client(
"display_atmosphere",
timeout=1,
config_callback=self.atm_config_callback)
#self.my_own_reconfigure_client = dynamic_reconfigure.client.Client("real_time_sim", timeout=1, config_callback=None)
self.cur_thermal_id = 0
self.dyn_cfg_srv = dyn_rec_srv.Server(ros_pat.cfg.guidanceConfig,
self.dyn_cfg_callback)
# we can not have two servers... f**k...
#self.atm_cfg_srv = dyn_rec_srv.Server(ros_pat.cfg.atmosphereConfig,
# self.atm_dyn_cfg_callback)#, subname='foo')
self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal",
geometry_msgs.msg.PoseStamped,
self.nav_goal_callback)
self.joy_sub = rospy.Subscriber('/joy', sensor_msgs.msg.Joy,
self.joy_callback)
self.periodic_cnt = 0
def __init__(self, ds):
self.ds = ds
self.tf_pub = p3_rpu.TransformPublisher()
self.marker_pub = p3_rpu.PoseArrayPublisher(dae='quad_murat.dae')
self.odom_pub = p3_rpu.OdomPublisher()
self.i=0