def __init__(self):
# Control states
self.NO_CONTROL = 0
self.EMERGENCY_BRAKE = 1
self.NORMAL_TRACKING = 2
self.ESTIMATE_TRACKING = 3
self.HIGH_SPEED = 3.
self.LOW_SPEED = 1.
# Control variable
self.look_ahead_distance = 50
self.look_ahead_point = PoseStamped()
# Input manage
self.vehicle_state = VehicleState()
self.estimated_path = Path()
self.latest_generated_path = Path()
self.current_path = Path()
self.update_path = False
self.latest_velocity_level = VelocityLevel()
self.velocity_level = VelocityLevel()
self.update_velocity_level = False
self.latest_motion_state = MotionState()
self.motion_state = MotionState()
self.update_motion_state = False
self.motion_state_buff = [self.motion_state]
self.motion_state_buff_size = 20
self.curvature = Curvature()
self.curvature.gear = 0
self.curvature_buff = [self.curvature]
self.curvature_time_buff = [rospy.get_rostime().to_sec()]
self.curvature_buff_size = 200
self.latest_vehicle_pose = Pose()
self.update_vehicle_pose = False
self.vehicle_pose = Pose()
self.temp_control_mode = 0
# Variable for parking
self.is_PARKING = False
self.init_parking_path = Path()
def __init__(self):
# Control constants
self.MAX_speed = 5.5 # [m/s]
self.MIN_speed = 0. # [m/s]
#self.MAX_steer = 2000/float(71) # [degree]
#self.MIN_steer = -2000/float(71) # [degree]
self.MAX_steer = 180.
self.MIN_steer = -180.
self.MAX_brake = 150
self.MIN_brake = 1
# Control states
self.NO_CONTROL = 0
self.EMERGENCY_BRAKE = 1
self.NORMAL_TRACKING = 2
self.ESTIMATE_TRACKING = 3
# Platform constants
self.wheelbase = 1.02 #[m] distance between two wheel axis
self.cm2rear = 0.51 #[m] distance between center of mass and rear axis
# Control variable
self.control = Control()
self.control_mode = self.NO_CONTROL
self.control_write = 0
self.control_count = 0
self.control_buff = []
self.control_time_buff = []
self.control_buff_size = 200
self.look_ahead_distance = 20
self.look_ahead_point = PoseStamped()
# Input manage
self.vehicle_state = Control()
self.latest_generated_path = Path()
self.current_path = Path()
self.update_path = False
self.latest_velocity_level = VelocityLevel()
self.velocity_level = VelocityLevel()
self.update_velocity_level = False
self.curvature = Curvature()
def __init__ (self) :
# Control states
self.NO_CONTROL = 0
self.EMERGENCY_BRAKE = 1
self.NORMAL_TRACKING = 2
self.ESTIMATE_TRACKING = 3
# Control variable
self.look_ahead_distance = 10
self.look_ahead_point= PoseStamped()
# Input manage
self.vehicle_state = VehicleState()
self.latest_generated_path = Path()
self.current_path = Path()
self.update_path = False
self.first_path = False
self.latest_velocity_level = VelocityLevel()
self.velocity_level = VelocityLevel()
self.update_velocity_level = False
self.latest_motion_state = MotionState()
self.motion_state = MotionState()
self.update_motion_state = False
self.motion_state_buff = []
self.motion_state_buff_size = 20
self.curvature = Curvature()
self.curvature.gear = 1
self.curvature_write = 0
self.curvature_count = 0
self.curvature_buff = []
self.curvature_time_buff = []
self.curvature_buff_size = 200
self.temp_control_mode = 0
# Variable for parking
self.is_PARKING = False
self.init_parking_path = Path()
def __init__(self):
self.lane_map = None
self.lidar_map = None
self.merged_map = None
self.left_cor = [0, 0] #upper bound of left lane
self.right_cor = [0, 0] #upper bound if right lane
self.mid_cor = [0, 0] #medium of two coors
self.turn_count = 0
self.count = 0
self.pose = PoseStamped()
self.velocity = VelocityLevel()
def mainloop():
'''
code for activate and deactivate the node
'''
nodename = 'local_map_generator'
mainloop.active = True
def signalResponse(data):
mainloop.active
if 'zero_monitor' in data.active_nodes:
if nodename in data.active_nodes:
mainloop.active = True
else:
mainloop.active = False
else:
rospy.signal_shutdown('no monitor')
rospy.Subscriber('/active_nodes', ActiveNode, signalResponse)
'''
...
'''
rospy.Subscriber("/motion_state", MotionState, mcb)
rospy.Subscriber("/raw_local_map", Image, rlmcb)
pubgoal = rospy.Publisher('/goal_pose', PoseStamped, queue_size=10)
pubmap = rospy.Publisher('/local_map', OccupancyGrid, queue_size=10)
pubtf = rospy.Publisher('/tf', TFMessage, queue_size=10)
pubvel = rospy.Publisher('/velocity_level', VelocityLevel, queue_size=10)
rospy.init_node(nodename, anonymous=True)
rate = rospy.Rate(10) # 10hz
goal = PoseStamped()
goal.pose.position.y = 2
goal.pose.orientation.w = 1
goal.header.frame_id = "car_frame"
vellv = VelocityLevel()
vellv.header.frame_id = "car_frame"
qmap = tf_conversions.transformations.quaternion_from_euler(0, 0, 0)
simplemap = OccupancyGrid()
simplemap.header.frame_id = "car_frame"
simplemap.info.resolution = 0.03 #0.5
simplemap.info.width = 200 #100
simplemap.info.height = 200 #100
simplemap.info.origin.position.x = -3 #-25
simplemap.info.origin.position.y = -0.5 #-25
simplemap.info.origin.orientation.x = qmap[0]
simplemap.info.origin.orientation.y = qmap[1]
simplemap.info.origin.orientation.z = qmap[2]
simplemap.info.origin.orientation.w = qmap[3]
simplemap.data = makemap(simplemap.info.height, simplemap.info.width)
qframe = tf_conversions.transformations.quaternion_from_euler(0, 0, 0)
simpletf = TFMessage()
simpletf.transforms.append(TransformStamped())
simpletf.transforms.append(TransformStamped())
simpletf.transforms[0].header.frame_id = "global_ref"
simpletf.transforms[0].child_frame_id = "car_frame"
simpletf.transforms[0].transform.rotation.x = qframe[0]
simpletf.transforms[0].transform.rotation.y = qframe[1]
simpletf.transforms[0].transform.rotation.z = qframe[2]
simpletf.transforms[0].transform.rotation.w = qframe[3]
simpletf.transforms[0].transform.translation.x = 0
simpletf.transforms[0].transform.translation.y = 0
simpletf.transforms[1].header.frame_id = "car_frame"
simpletf.transforms[1].child_frame_id = "camera"
simpletf.transforms[1].transform.rotation.x = qframe[0]
simpletf.transforms[1].transform.rotation.y = qframe[1]
simpletf.transforms[1].transform.rotation.z = qframe[2]
simpletf.transforms[1].transform.rotation.w = qframe[3]
simpletf.transforms[1].transform.translation.x = 1
simpletf.transforms[1].transform.translation.y = 2
i = 0
while not rospy.is_shutdown():
simplemap.header.stamp = rospy.Time.now()
simplemap.header.seq = i
goal.header.stamp = rospy.Time.now()
goal.header.seq = i
vellv.header.stamp = rospy.Time.now()
vellv.header.seq = i
simpletf.transforms[0].header.stamp = rospy.Time.now()
simpletf.transforms[0].header.seq = i
simpletf.transforms[1].header.stamp = rospy.Time.now()
simpletf.transforms[1].header.seq = i
i = i + 1
if mainloop.active:
pubgoal.publish(goal)
pubmap.publish(simplemap)
pubtf.publish(simpletf)
pubvel.publish(vellv)
rate.sleep()
np.flipud(merged_arr).ravel().astype(np.int8))
occupancy.header.stamp = rospy.Time.now()
occupancy.header.frame_id = "car_frame"
occupancy.info.resolution = 0.03
occupancy.info.width = merged_arr.shape[0]
occupancy.info.height = merged_arr.shape[1]
occupancy.info.origin.position.x = -occupancy.info.width / 2 * occupancy.info.resolution
occupancy.info.origin.position.y = 0
occupancy.info.origin.position.z = 0
occupancy.info.origin.orientation.z = 0
occupancy.info.origin.orientation.w = 1
if isactive:
obstacle_map_pub.publish(merged_img)
if isactive:
local_map_pub.publish(occupancy)
pose = PoseStamped()
pose.header.frame_id = "car_frame"
if isactive:
goal_pose_pub.publish(pose)
velocity = VelocityLevel()
velocity.header.frame_id = "car_frame"
if isactive:
velocity_level_pub.publish(velocity)
rate.sleep()
#rospy.spin()
#except TypeError:
# rospy.loginfo("None type is in either lane_map or lidar_map")