def odomCB(self, odo): self.robot_odom = odo if not self.list_cleaning: self.odom_list[0].append(odo.header.stamp) self.odom_list[1].append(odo.pose.pose) if self.odom_calibrating: self.rate.sleep() return try: trans = self.tfBuffer.lookup_transform(self.gps_frame, self.odom_frame, rospy.Time()) if self.verify_stamp > trans.header.stamp: debug_info(self.debug_output, "Looking up transformations") return robot_odom_pose = PoseStamped() robot_odom_pose.pose = self.robot_odom.pose.pose pose_transformed = tf2_geometry_msgs.do_transform_pose(robot_odom_pose, trans) robot_gps_pose = Odometry() robot_gps_pose.header.frame_id = self.gps_frame robot_gps_pose.child_frame_id = self.robot_frame robot_gps_pose.pose.pose = pose_transformed.pose robot_gps_pose.pose.pose.position.x,robot_gps_pose.pose.pose.position.y = self.projection(pose_transformed.pose.position.x,pose_transformed.pose.position.y,inverse=True) robot_gps_pose.pose.pose.orientation.x = -pose_transformed.pose.orientation.x robot_gps_pose.pose.pose.orientation.y = -pose_transformed.pose.orientation.y robot_gps_pose.pose.pose.orientation.z = -pose_transformed.pose.orientation.z robot_gps_pose.pose.pose.orientation = quat_rot(robot_gps_pose.pose.pose.orientation,0,0,90) self.robot_gps_pub.publish(robot_gps_pose) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): return
def poseCB(self, p): self.odom_calibrating = True robot_pose = Pose() robot_pose = p.pose.pose robot_pose.position.x, robot_pose.position.y = self.projection( p.pose.pose.position.x, p.pose.pose.position.y) robot_pose.orientation.x = -p.pose.pose.orientation.x robot_pose.orientation.y = -p.pose.pose.orientation.y robot_pose.orientation.z = -p.pose.pose.orientation.z robot_pose.orientation = quat_rot(robot_pose.orientation, 0, 0, 90) debug_info(self.debug_output, "Odom calib tf received") # odom to reference while 1: try: odo_ref_trans = self.tfBuffer.lookup_transform( self.robot_frame, self.odom_frame, rospy.Time()) tf_odo_ref = TransformStamped() tf_odo_ref.header.frame_id = "odom_utm_calib" tf_odo_ref.child_frame_id = self.odom_frame tf_odo_ref.header.stamp = rospy.Time.now() tf_odo_ref.transform = odo_ref_trans.transform tfmsg_odo_ref = tf2_msgs.msg.TFMessage([tf_odo_ref]) self.tf_pub.publish(tfmsg_odo_ref) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue # reference to utm tf_ref_utm = TransformStamped() tf_ref_utm.header.frame_id = self.gps_frame tf_ref_utm.child_frame_id = "odom_utm_calib" tf_ref_utm.header.stamp = rospy.Time.now() tf_ref_utm.transform.translation.x = robot_pose.position.x tf_ref_utm.transform.translation.y = robot_pose.position.y tf_ref_utm.transform.translation.z = robot_pose.position.z tf_ref_utm.transform.rotation = robot_pose.orientation tfmsg_ref_utm = tf2_msgs.msg.TFMessage([tf_ref_utm]) self.tf2_pub.publish(tfmsg_ref_utm) self.rate.sleep() # Check the tf exists correctly try: trans = self.tfBuffer.lookup_transform(self.gps_frame, "odom_utm_calib", rospy.Time()) trans2 = self.tfBuffer.lookup_transform( "odom_utm_calib", self.odom_frame, rospy.Time()) if math.fabs(trans.transform.translation.x - robot_pose.position.x) > 0.1 or math.fabs( trans.transform.translation.y - robot_pose.position.y) > 0.1: continue self.odom_calibrating = False debug_info(self.debug_output, "Odometry calibrated") break except: continue
def goalCB(self, g): self.goal_set = False x, y = self.projection(g.longitude, g.latitude) z = g.altitude if not self.pose_get: self.robot_pose.position.x = x self.robot_pose.position.y = y self.robot_pose.position.z = z self.pose_get = True return self.goal.x = x self.goal.y = y self.goal.z = z self.waypoint = g debug_info(self.debug_output, "Waypoint received") if self.orentation_get: return dx = x - self.robot_pose.position.x dy = y - self.robot_pose.position.y dz = z - self.robot_pose.position.z robot_roll = 0 robot_pitch = math.atan2(dz, math.sqrt(dx**2 + dy**2)) robot_yaw = math.atan2(dy, dx) robot_quat = tf.transformations.quaternion_from_euler( robot_roll, robot_pitch, robot_yaw) self.robot_pose.orientation.x = robot_quat[0] self.robot_pose.orientation.y = robot_quat[1] self.robot_pose.orientation.z = robot_quat[2] self.robot_pose.orientation.w = robot_quat[3] self.orentation_get = True # Publish robot initial position to calibrate the Odometry robot_gps_pose = Odometry() robot_gps_pose.pose.pose = self.robot_pose robot_gps_pose.pose.pose.position.x, robot_gps_pose.pose.pose.position.y = self.projection( self.robot_pose.position.x, self.robot_pose.position.y, inverse=True) robot_gps_pose.pose.pose.orientation.x = -self.robot_pose.orientation.x robot_gps_pose.pose.pose.orientation.y = -self.robot_pose.orientation.y robot_gps_pose.pose.pose.orientation.z = -self.robot_pose.orientation.z robot_gps_pose.pose.pose.orientation = quat_rot( robot_gps_pose.pose.pose.orientation, 0, 0, 90) self.robot_gps_pub.publish(robot_gps_pose)
def accCB(self, a): if len(a.data) <= 0: debug_info(self.debug_output, "waypoint_control acceleration invalid") return parts = a.data.split(',') if len(parts) != 2: debug_info(self.debug_output, "waypoint_control error: 2 accelerations needed, only " + str(len(parts)) + " sent") return try: self.ACC = float(parts[0]) self.ACC_R = float(parts[1]) debug_info(self.debug_output, "Acceleration updated: " + str(self.ACC) +", " + str(self.ACC_R)) except: return
def paraCB(self, p): if len(p.data) <= 0: debug_info(self.debug_output, "odometry_control parameter invalid") return parts = p.data.split(',') if len(parts) != 4: debug_info(self.debug_output, "odometry_control error: 4 parameters needed, only " + str(len(parts)) + " sent") return try: self.K_RHO = float(parts[0]) self.K_ROLL = float(parts[1]) self.K_PITCH = float(parts[2]) self.K_YAW = float(parts[3]) debug_info(self.debug_output, "Parameter updated: " + str(self.K_RHO) +", " + str(self.K_ROLL) +", " + str(self.K_PITCH) +", " + str(self.K_YAW)) except: return
def __init__(self): # Variables self.projection = Proj(proj="utm", zone="34", ellps='WGS84') self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer) self.reference_id = 0 self.fiducial_gps_map = FiducialMapEntryArray() self.state = "STOP" self.prestate = "STOP" self.previous_fiducial = 0 self.robot_stopped = True self.waiting_time = 0 self.waiting = False self.displacement = 0 self.pre_odom = Odometry() self.pre_odom_get = False # Init ROS node rospy.init_node('fiducial_waypoint_localization') # rosparams self.robot_frame = rospy.get_param("~waypoint_control/base_frame", "base_footprint") self.gps_frame = rospy.get_param("~waypoint_control/gps_frame", "utm") self.camera_frame = rospy.get_param("~waypoint_control/camera_frame", "raspicam") self.trackWidth = float( rospy.get_param("~driveGeometry/trackWidth", "0.403")) self.fiducial_map_file = rospy.get_param("~waypoint_control/map_file", "Fiducials.json") # Publishers self.robot_gps_pub = rospy.Publisher('robot_gps_pose', Odometry, queue_size=10, latch=True) self.tf_pub = rospy.Publisher("tf", tf2_msgs.msg.TFMessage, queue_size=30, latch=True) self.state_pub = rospy.Publisher('waypoint/state', String, queue_size=10, latch=True) self.debug_output = rospy.Publisher('debug_output', String, queue_size=10) # Subscribers rospy.Subscriber('fiducial_map_gps', FiducialMapEntryArray, self.mapGPSCB) rospy.Subscriber('fiducial_transforms', FiducialTransformArray, self.transCB) rospy.Subscriber('waypoint/state', String, self.stateCB) rospy.Subscriber('odom', Odometry, self.odomCB) # 1 Hz self.rate = rospy.Rate(1) # Init fiducials map in GPS from json, publish all fiducial to utm trans to tf try: json_data = json.load(open(self.fiducial_map_file)) # Save the map in FiducialMapEntryArray() json_map = FiducialMapEntryArray() for fid in json_data["FiducialCollections"][0]["SavedFiducials"]: fid_gps = FiducialMapEntry() fid_gps.fiducial_id = fid["Id"] fid_gps.x = fid["Position"]["longitude"] fid_gps.y = fid["Position"]["latitude"] fid_gps.z = fid["Position"]["altitude"] fid_gps.rx = fid["Rotation"]["east"] fid_gps.ry = fid["Rotation"]["north"] fid_gps.rz = fid["Rotation"]["heading"] json_map.fiducials.append(fid_gps) self.fiducial_gps_map = json_map debug_info(self.debug_output, "Fiducial map initialised") except: debug_info(self.debug_output, "Initialise map from json file failed")
def transCB(self, t): prev_in_view = False reference_in_view = False for fid_trans in t.transforms: # Check whether the detected fiducial is still in the view if fid_trans.fiducial_id == self.reference_id: reference_in_view = True # Check whether the previous fiducial is out of the view if fid_trans.fiducial_id == self.previous_fiducial: prev_in_view = True if self.displacement < 3: return # If the detected is out of view when the robot stops, continue if self.waiting and not reference_in_view: # Cotinue move to the waypoint self.waiting = False state_msg = String() state_msg.data = self.prestate self.state_pub.publish(state_msg) debug_info(self.debug_output, "Fiducial detected is out of the view") return for fid_trans in t.transforms: # Check the image error if fid_trans.image_error > 0.3: return # Check whether detected fiducial is in the map for fid_gps in self.fiducial_gps_map.fiducials: # Only process the detected fiducial in the map if fid_trans.fiducial_id != fid_gps.fiducial_id: continue self.reference_id = fid_trans.fiducial_id fid_name = str(self.reference_id) # Pause the navigation and stop the robot if not self.waiting: self.waiting = True self.prestate = self.state debug_info(self.debug_output, "Reading fiducial ...") state_msg = String() state_msg.data = "STOP" self.state_pub.publish(state_msg) # wait until robot stops if not self.robot_stopped: # Initialise the waiting time self.waiting_time = 0 self.rate.sleep() debug_info(self.debug_output, "Wait until robot stop ...") return # Wait another 5 seconds for updating the image if self.waiting_time < 5: self.waiting_time += 1 self.rate.sleep() debug_info(self.debug_output, "Wait another 5 seconds ...") return # Publish tf from fid to camera tf_fid_cam = TransformStamped() tf_fid_cam.header.frame_id = self.camera_frame tf_fid_cam.child_frame_id = fid_name tf_fid_cam.header.stamp = rospy.Time.now() tf_fid_cam.transform = fid_trans.transform tfmsg_fid_cam = tf2_msgs.msg.TFMessage([tf_fid_cam]) self.tf_pub.publish(tfmsg_fid_cam) if self.robot_stopped: self.rate.sleep() # Publish tf from fid to utm tf_fid_utm = TransformStamped() tf_fid_utm.header.frame_id = self.gps_frame tf_fid_utm.child_frame_id = "fiducial" + fid_name tf_fid_utm.header.stamp = rospy.Time.now() tf_fid_utm.transform.translation.x, tf_fid_utm.transform.translation.y = self.projection( fid_gps.x, fid_gps.y) tf_fid_utm.transform.translation.z = fid_gps.z quat = tf.transformations.quaternion_from_euler( -math.radians(fid_gps.rx), -math.radians(fid_gps.ry), -math.radians(fid_gps.rz)) tf_fid_utm.transform.rotation.x = quat[0] tf_fid_utm.transform.rotation.y = quat[1] tf_fid_utm.transform.rotation.z = quat[2] tf_fid_utm.transform.rotation.w = quat[3] tfmsg_fid_utm = tf2_msgs.msg.TFMessage([tf_fid_utm]) self.tf_pub.publish(tfmsg_fid_utm) if self.robot_stopped: self.rate.sleep() # Publish tf from robot to fid try: robot_fid_trans = self.tfBuffer.lookup_transform( fid_name, self.robot_frame, rospy.Time()) tf_robot_fid = TransformStamped() tf_robot_fid.header.frame_id = "fiducial" + fid_name tf_robot_fid.child_frame_id = "robot_fid" tf_robot_fid.header.stamp = rospy.Time.now() tf_robot_fid.transform = robot_fid_trans.transform tfmsg_robot_fid = tf2_msgs.msg.TFMessage([tf_robot_fid]) self.tf_pub.publish(tfmsg_robot_fid) if self.robot_stopped: self.rate.sleep() except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): debug_info(self.debug_output, "Creating tf from robot to fid") return # Publish tf from robot to utm try: verify_trans = self.tfBuffer.lookup_transform( self.gps_frame, "fiducial" + fid_name, rospy.Time()) verify_trans2 = self.tfBuffer.lookup_transform( "fiducial" + fid_name, "robot_fid", rospy.Time()) robot_utm_trans = self.tfBuffer.lookup_transform( self.gps_frame, "robot_fid", rospy.Time()) robot_gps_pose = Odometry() robot_gps_pose.pose.pose.position.z = robot_utm_trans.transform.translation.z robot_gps_pose.pose.pose.position.x, robot_gps_pose.pose.pose.position.y = self.projection( robot_utm_trans.transform.translation.x, robot_utm_trans.transform.translation.y, inverse=True) robot_gps_pose.pose.pose.orientation.x = -robot_utm_trans.transform.rotation.x robot_gps_pose.pose.pose.orientation.y = -robot_utm_trans.transform.rotation.y robot_gps_pose.pose.pose.orientation.z = -robot_utm_trans.transform.rotation.z robot_gps_pose.pose.pose.orientation.w = robot_utm_trans.transform.rotation.w robot_gps_pose.pose.pose.orientation = quat_rot( robot_gps_pose.pose.pose.orientation, 0, 0, 90) self.robot_gps_pub.publish(robot_gps_pose) # Resume the navigation when the update is done state_msg = String() state_msg.data = self.prestate self.state_pub.publish(state_msg) self.waiting = False self.previous_fiducial = self.reference_id self.displacement = 0 debug_info(self.debug_output, "Fiducial position updated") except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): debug_info(self.debug_output, "Updating the position") return
def flyThresCB(self, thres): self.FLYING_THRES = thres.data debug_info(self.debug_output, "Flying threshold is set to: " + str(self.FLYING_THRES))
def trunThresCB(self, thres): self.TURNING_THRES = thres.data debug_info(self.debug_output, "Turning threshold is set to: " + str(self.TURNING_THRES))
def fwdThresCB(self, thres): self.FORWARDING_THRES = thres.data debug_info( self.debug_output, "Forwarding threshold is set to: " + str(self.FORWARDING_THRES))
def angCB(self, a): self.VEL_MAX_ANG = a.data debug_info(self.debug_output, "Max angular speed is set to: " + str(self.VEL_MAX_ANG))
def linCB(self, l): self.VEL_MAX_LIN = l.data debug_info(self.debug_output, "Max linear speed is set to: " + str(self.VEL_MAX_LIN))
def poseCB(self, p): self.odom_calibrating = True robot_pose = Pose() robot_pose = p.pose.pose robot_pose.position.x, robot_pose.position.y = self.projection(p.pose.pose.position.x, p.pose.pose.position.y) robot_pose.orientation.x = -p.pose.pose.orientation.x robot_pose.orientation.y = -p.pose.pose.orientation.y robot_pose.orientation.z = -p.pose.pose.orientation.z robot_pose.orientation = quat_rot(robot_pose.orientation,0,0,90) debug_info(self.debug_output, "Odom calib tf received") # Align odometry with odom_calib and calculate offset current_stamp = p.header.stamp if len(self.odom_list[1]) <= 0: return i = bisect_left(self.odom_list[0], current_stamp) if i >= len(self.odom_list[1]): i = len(self.odom_list[1]) - 1 current_euler = tf.transformations.euler_from_quaternion((self.robot_odom.pose.pose.orientation.x, self.robot_odom.pose.pose.orientation.y, self.robot_odom.pose.pose.orientation.z, self.robot_odom.pose.pose.orientation.w)) bench_euler = tf.transformations.euler_from_quaternion((self.odom_list[1][i].orientation.x, self.odom_list[1][i].orientation.y, self.odom_list[1][i].orientation.z, self.odom_list[1][i].orientation.w)) offset_odom_x = self.robot_odom.pose.pose.position.x - self.odom_list[1][i].position.x offset_odom_y = self.robot_odom.pose.pose.position.y - self.odom_list[1][i].position.y offset_odom_z = self.robot_odom.pose.pose.position.z - self.odom_list[1][i].position.z offset_odom_rz = current_euler[2] - bench_euler[2] # Remove out dated elements after calculating offset self.list_cleaning = True self.odom_list[0] = self.odom_list[0][i+1:] self.odom_list[1] = self.odom_list[1][i+1:] self.list_cleaning = False try: odo_utm_trans = self.tfBuffer.lookup_transform(self.gps_frame, self.odom_frame, rospy.Time()) odo_utm_euler = tf.transformations.euler_from_quaternion((odo_utm_trans.transform.rotation.x, odo_utm_trans.transform.rotation.y, odo_utm_trans.transform.rotation.z, odo_utm_trans.transform.rotation.w)) theta = odo_utm_euler[2] robot_pose.position.x += offset_odom_x * math.cos(theta) - offset_odom_y * math.sin(theta) robot_pose.position.y += offset_odom_x * math.sin(theta) + offset_odom_y * math.cos(theta) robot_pose.position.z += offset_odom_z robot_pose.orientation = quat_rot(robot_pose.orientation,0,0,math.degrees(offset_odom_rz)) debug_info(self.debug_output, "Odometry offset calculated") except: debug_info(self.debug_output, "Initialise odometry frame in utm") # odom to reference while True: try: odo_ref_trans = self.tfBuffer.lookup_transform(self.robot_frame, self.odom_frame, rospy.Time()) tf_odo_ref = TransformStamped() tf_odo_ref.header.frame_id = "odom_utm_calib" tf_odo_ref.child_frame_id = self.odom_frame tf_odo_ref.header.stamp = rospy.Time.now() tf_odo_ref.transform = odo_ref_trans.transform tfmsg_odo_ref = tf2_msgs.msg.TFMessage([tf_odo_ref]) self.tf_pub.publish(tfmsg_odo_ref) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): debug_info(self.debug_output, "Publishing odometry calibration") continue # reference to utm tf_ref_utm = TransformStamped() tf_ref_utm.header.frame_id = self.gps_frame tf_ref_utm.child_frame_id = "odom_utm_calib" tf_ref_utm.header.stamp = rospy.Time.now() tf_ref_utm.transform.translation.x = robot_pose.position.x tf_ref_utm.transform.translation.y = robot_pose.position.y tf_ref_utm.transform.translation.z = robot_pose.position.z tf_ref_utm.transform.rotation = robot_pose.orientation tfmsg_ref_utm = tf2_msgs.msg.TFMessage([tf_ref_utm]) self.tf2_pub.publish(tfmsg_ref_utm) self.rate.sleep() # Check the tf exists correctly try: trans = self.tfBuffer.lookup_transform(self.gps_frame, "odom_utm_calib", rospy.Time()) trans2 = self.tfBuffer.lookup_transform("odom_utm_calib", self.odom_frame, rospy.Time()) if math.fabs(trans.transform.translation.x - robot_pose.position.x) > 0.1 or math.fabs(trans.transform.translation.y - robot_pose.position.y) > 0.1: continue self.odom_calibrating = False debug_info(self.debug_output, "Odometry calibrated") break except: continue self.verify_stamp = tf_odo_ref.header.stamp
def transCB(self, t): for fid_trans in t.transforms: # Check the image error if fid_trans.image_error > 0.3: return # Check whether detected fiducial is in the map for fid_gps in self.fiducial_gps_map.fiducials: # Only process the detected fiducial in the map if fid_trans.fiducial_id != fid_gps.fiducial_id: continue self.reference_id = fid_trans.fiducial_id fid_name = str(self.reference_id) # Keep publishing tf until it is created while True: # Publish tf from fid to camera tf_fid_cam = TransformStamped() tf_fid_cam.header.frame_id = self.camera_frame tf_fid_cam.child_frame_id = fid_name tf_fid_cam.header.stamp = rospy.Time.now() tf_fid_cam.transform = fid_trans.transform tfmsg_fid_cam = tf2_msgs.msg.TFMessage([tf_fid_cam]) self.tf_pub.publish(tfmsg_fid_cam) # Publish tf from fid to utm tf_fid_utm = TransformStamped() tf_fid_utm.header.frame_id = self.gps_frame tf_fid_utm.child_frame_id = "fiducial" + fid_name tf_fid_utm.header.stamp = rospy.Time.now() tf_fid_utm.transform.translation.x, tf_fid_utm.transform.translation.y = self.projection( fid_gps.x, fid_gps.y) tf_fid_utm.transform.translation.z = fid_gps.z quat = tf.transformations.quaternion_from_euler( -math.radians(fid_gps.rx), -math.radians(fid_gps.ry), -math.radians(fid_gps.rz)) tf_fid_utm.transform.rotation.x = quat[0] tf_fid_utm.transform.rotation.y = quat[1] tf_fid_utm.transform.rotation.z = quat[2] tf_fid_utm.transform.rotation.w = quat[3] tfmsg_fid_utm = tf2_msgs.msg.TFMessage([tf_fid_utm]) self.tf_pub.publish(tfmsg_fid_utm) # Publish tf from robot to fid try: robot_fid_trans = self.tfBuffer.lookup_transform( fid_name, self.robot_frame, rospy.Time()) tf_robot_fid = TransformStamped() tf_robot_fid.header.frame_id = "fiducial" + fid_name tf_robot_fid.child_frame_id = "robot_fid" tf_robot_fid.header.stamp = rospy.Time.now() tf_robot_fid.transform = robot_fid_trans.transform tfmsg_robot_fid = tf2_msgs.msg.TFMessage( [tf_robot_fid]) self.tf_pub.publish(tfmsg_robot_fid) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): debug_info(self.debug_output, "Creating tf from robot to fid") continue # Publish tf from robot to utm try: verify_stamp = tf_fid_utm.header.stamp robot_utm_trans = self.tfBuffer.lookup_transform( self.gps_frame, "robot_fid", rospy.Time()) if verify_stamp > robot_utm_trans.header.stamp: debug_info(self.debug_output, "Looking up transformations") continue robot_gps_pose = Odometry() robot_gps_pose.header.stamp = t.header.stamp # Important to apply offset robot_gps_pose.pose.pose.position.z = robot_utm_trans.transform.translation.z robot_gps_pose.pose.pose.position.x, robot_gps_pose.pose.pose.position.y = self.projection( robot_utm_trans.transform.translation.x, robot_utm_trans.transform.translation.y, inverse=True) robot_gps_pose.pose.pose.orientation.x = -robot_utm_trans.transform.rotation.x robot_gps_pose.pose.pose.orientation.y = -robot_utm_trans.transform.rotation.y robot_gps_pose.pose.pose.orientation.z = -robot_utm_trans.transform.rotation.z robot_gps_pose.pose.pose.orientation.w = robot_utm_trans.transform.rotation.w robot_gps_pose.pose.pose.orientation = quat_rot( robot_gps_pose.pose.pose.orientation, 0, 0, 90) self.robot_gps_pub.publish(robot_gps_pose) debug_info(self.debug_output, "Fiducial position updated") break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): debug_info(self.debug_output, "Updating the position") continue