def gen_1lap_path_1ll(lon0, lat0, deg, dist, turn_r=1.5):
x0, y0 = ll2xy(lon0, lat0)
rad1 = deg2rad(deg)
rad2 = rad1 + pi
x1 = x0 + dist * cos(rad1)
y1 = y0 + dist * sin(rad1)
x2 = x0 + dist * cos(rad2)
y2 = y0 + dist * sin(rad2)
return gen_1lap_path_3pt(x0, y0, x1, y1, x2, y2, turn_r=turn_r)
def cb_R(sol_R):
global R_last_t,Rx,Ry, R_old,R_new
R_last_t=rospy.get_time()
R_old=R_new
R_new=sol_R
Rx,Ry=ll2xy(sol_R.longitude, sol_R.latitude)
# if work_origin was loaded, offset to origin
Rx = Rx - work_origin.x
Ry = Ry - work_origin.y
# pprint(sol_R)
if L_new==None:
return
if R_last_t-L_last_t<0.1 and R_last_t>L_last_t:
rospy.logdebug("R %.1f"%(R_last_t-t0))
calc_pose()
def cb_L(sol_L):
global L_last_t,Lx,Ly, L_old,L_new
L_last_t=rospy.get_time()
L_old=L_new
L_new=sol_L
Lx,Ly=ll2xy(sol_L.longitude, sol_L.latitude)
# if work_origin was loaded, offset to origin
Lx = Lx - work_origin.x
Ly = Ly - work_origin.y
# pprint(sol_L)
if R_new==None:
return
if L_last_t-R_last_t<0.1 and L_last_t>R_last_t:
rospy.logdebug("L %.1f"%(L_last_t-t0))
calc_pose()
def cb_R(self, sol_R):
self.R_last_t = rospy.get_time()
self.R_old = self.R_new
self.R_new = sol_R
self.R_fix_stat = self.R_new.position_covariance_type
if self.R_fix_stat==1:
self.R_single_time += self.period_assumed
elif self.R_fix_stat==2:
self.R_float_time += self.period_assumed
elif self.R_fix_stat==3:
self.R_fix_time += self.period_assumed
self.Rx, self.Ry = ll2xy(sol_R.longitude, sol_R.latitude)
# if work_origin was loaded, offset to origin
self.Rx = self.Rx - self.work_origin.x
self.Ry = self.Ry - self.work_origin.y
#pprint(sol_R)
if self.L_new==None:
return
if self.R_last_t - self.L_last_t<0.1 and self.R_last_t > self.L_last_t:
# rospy.logdebug("R %.1f"%(self.R_last_t - self.t0))
self.calc_pose()
from utm_coordinate import ll2xy,\
UTM_FRAME_NAME,\
WORK_ORIGIN_TOPIC_NAME,\
WORK_ORIGIN_FRAME_NAME
from course_location import LON0,LAT0
import rospy
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions
#from tf.transformations import euler_from_quaternion, quaternion_from_euler
from geometry_msgs.msg import Pose2D
if __name__ == '__main__':
center = ll2xy(LON0,LAT0)
# ROS ノードの初期化処理
rospy.init_node('work_offset_broadcaster')
# ブロードキャスタ、Transform
br = tf2_ros.StaticTransformBroadcaster()
t = TransformStamped()
# Transform の時刻情報、Base となる座標系、world を Base とする座標系
t.header.stamp = rospy.Time.now()
t.header.frame_id = UTM_FRAME_NAME
t.child_frame_id = WORK_ORIGIN_FRAME_NAME
# 6D pose (位置 translation、姿勢 rotation)
t.transform.translation.x = center[0]
def gen_1lap_path_2ll(lon1, lat1, lon2, lat2, turn_r=1.5):
x1, y1 = ll2xy(lon1, lat1)
x2, y2 = ll2xy(lon2, lat2)
x0 = (x1 + x2) / 2.
y0 = (y1 + y2) / 2.
return gen_1lap_path_3pt(x0, y0, x1, y1, x2, y2, turn_r=turn_r)
def gen_1lap_path_3ll(lon0, lat0, lon1, lat1, lon2, lat2, turn_r=1.5):
x0, y0 = ll2xy(lon0, lat0)
x1, y1 = ll2xy(lon1, lat1)
x2, y2 = ll2xy(lon2, lat2)
return gen_1lap_path_3pt(x0, y0, x1, y1, x2, y2, turn_r=turn_r)