localization.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 __author__ = 'lsouchet'
00003 
00004 
00005 # Copyright (C) 2014 Aldebaran Robotics
00006 #
00007 # Licensed under the Apache License, Version 2.0 (the "License");
00008 # you may not use this file except in compliance with the License.
00009 # You may obtain a copy of the License at
00010 #
00011 #     http://www.apache.org/licenses/LICENSE-2.0
00012 #
00013 # Unless required by applicable law or agreed to in writing, software
00014 # distributed under the License is distributed on an "AS IS" BASIS,
00015 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00016 # See the License for the specific language governing permissions and
00017 # limitations under the License.
00018 #
00019 
00020 #import ROS dependencies
00021 import rospy
00022 from geometry_msgs.msg import Point32
00023 from visualization_msgs.msg import Marker
00024 from geometry_msgs.msg import PoseStamped
00025 from nav_msgs.msg import Path
00026 
00027 #import NAO dependencies
00028 from naoqi_driver.naoqi_node import NaoqiNode
00029 from geometry_msgs.msg import PoseArray
00030 from geometry_msgs.msg import Pose
00031 from geometry_msgs.msg import Point
00032 from geometry_msgs.msg import Quaternion
00033 from tf2_msgs.msg import TFMessage
00034 from geometry_msgs.msg import TransformStamped
00035 from geometry_msgs.msg import Vector3
00036 from sensor_msgs.msg import LaserScan
00037 from std_msgs.msg import ColorRGBA
00038 from nav_msgs.msg import OccupancyGrid
00039 import almath
00040 
00041 class ParticlesPublisher(NaoqiNode):
00042 
00043     def __init__(self):
00044         NaoqiNode.__init__(self, 'naoqi_localization')
00045         self.navigation = None
00046         self.connectNaoQi()
00047         self.publishers = {}
00048         self.publishers["uncertainty"] = rospy.Publisher('localization_uncertainty', Marker)
00049         self.publishers["map_tf"] = rospy.Publisher('/tf', TFMessage, latch=True)
00050         self.publishers["map"] = rospy.Publisher('naoqi_exploration_map', OccupancyGrid , queue_size=None)
00051         self.publishers["exploration_path"] = rospy.Publisher('naoqi_exploration_path', Path, queue_size=None)
00052         self.frequency = 2
00053         self.rate = rospy.Rate(self.frequency)
00054 
00055     # (re-) connect to NaoQI:
00056     def connectNaoQi(self):
00057         rospy.loginfo("Exploration Node Connecting to NaoQi at %s:%d", self.pip, self.pport)
00058 
00059         self.navigation = self.get_proxy("ALNavigation")
00060         self.motion = self.get_proxy("ALMotion")
00061         if self.navigation is None or self.motion is None:
00062             rospy.logerr("Unable to reach ALMotion and ALNavigation.")
00063             exit(0)
00064         version_array = self.get_proxy("ALSystem").systemVersion().split('.')
00065         if len(version_array) < 3:
00066             rospy.logerr("Unable to deduce the system version.")
00067             exit(0)
00068         version_tuple = (int(version_array[0]), int(version_array[1]), int(version_array[2]))
00069         min_version = (2, 5, 2)
00070         if version_tuple < min_version:
00071             rospy.logerr("Naoqi version " + str(min_version) + " required for localization. Yours is " + str(version_tuple))
00072             exit(0)
00073 
00074 
00075     def get_ros_quaternion(self, almath_quaternion):
00076         output = Quaternion()
00077         output.w = almath_quaternion.w
00078         output.x = almath_quaternion.x
00079         output.y = almath_quaternion.y
00080         output.z = almath_quaternion.z
00081         return output
00082 
00083     def get_navigation_tf(self, navigation_pose):
00084         navigation_tf = TransformStamped()
00085         navigation_tf.header.frame_id = "/map"
00086         navigation_tf.header.stamp = rospy.Time.now()
00087         navigation_tf.child_frame_id = "/odom"
00088         navigation_tf.transform.translation .x = navigation_pose.x
00089         navigation_tf.transform.translation .y = navigation_pose.y
00090         navigation_tf.transform.rotation = self.get_ros_quaternion(
00091                     almath.Quaternion_fromAngleAndAxisRotation(navigation_pose.theta, 0, 0, 1))
00092         return navigation_tf
00093 
00094     def build_laser_scan(self, ranges):
00095         result = LaserScan()
00096         result.header.stamp = rospy.Time.now()
00097         result.angle_min = -almath.PI
00098         result.angle_max = almath.PI
00099         if len(ranges[1]) > 0:
00100             result.angle_increment = (result.angle_max - result.angle_min) / len(ranges[1])
00101         result.range_min = 0.0
00102         result.range_max = ranges[0]
00103         for range_it in ranges[1]:
00104             result.ranges.append(range_it[1])
00105         return result
00106 
00107     # do it!
00108     def run(self):
00109         while self.is_looping():
00110             navigation_tf_msg = TFMessage()
00111             try:
00112                 motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True))
00113                 localization = self.navigation.getRobotPositionInMap()
00114                 exploration_path = self.navigation.getExplorationPath()
00115             except Exception as e:
00116                 navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D()))
00117                 self.publishers["map_tf"].publish(navigation_tf_msg)
00118                 self.rate.sleep()
00119                 continue
00120             if len(localization) > 0 and len(localization[0]) == 3:
00121                 navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2])
00122                 navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot)
00123                 navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion))
00124             self.publishers["map_tf"].publish(navigation_tf_msg)
00125             if len(localization) > 0:
00126                 if self.publishers["uncertainty"].get_num_connections() > 0:
00127                     uncertainty = Marker()
00128                     uncertainty.header.frame_id = "/base_footprint"
00129                     uncertainty.header.stamp = rospy.Time.now()
00130                     uncertainty.type = Marker.CYLINDER
00131                     uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2)
00132                     uncertainty.pose.position = Point(0, 0, 0)
00133                     uncertainty.pose.orientation = self.get_ros_quaternion(
00134                         almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1))
00135                     uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5)
00136                     self.publishers["uncertainty"].publish(uncertainty)
00137             if self.publishers["map"].get_num_connections() > 0:
00138                 aggregated_map = None
00139                 try:
00140                     aggregated_map = self.navigation.getMetricalMap()
00141                 except Exception as e:
00142                     print("error " + str(e))
00143                 if aggregated_map is not None:
00144                     map_marker = OccupancyGrid()
00145                     map_marker.header.stamp = rospy.Time.now()
00146                     map_marker.header.frame_id = "/map"
00147                     map_marker.info.resolution = aggregated_map[0]
00148                     map_marker.info.width = aggregated_map[1]
00149                     map_marker.info.height = aggregated_map[2]
00150                     map_marker.info.origin.orientation = self.get_ros_quaternion(
00151                         almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1))
00152                     map_marker.info.origin.position.x = aggregated_map[3][0]
00153                     map_marker.info.origin.position.y = aggregated_map[3][1]
00154                     map_marker.data = aggregated_map[4]
00155                     self.publishers["map"].publish(map_marker)
00156             if self.publishers["exploration_path"].get_num_connections() > 0:
00157                 path = Path()
00158                 path.header.stamp = rospy.Time.now()
00159                 path.header.frame_id = "/map"
00160                 for node in exploration_path:
00161                     current_node = PoseStamped()
00162                     current_node.pose.position.x = node[0]
00163                     current_node.pose.position.y = node[1]
00164                     path.poses.append(current_node)
00165                 self.publishers["exploration_path"].publish(path)
00166             self.rate.sleep()
00167 
00168 if __name__ == '__main__':
00169     pub = ParticlesPublisher()
00170     pub.start()
00171 
00172     rospy.spin()
00173     exit(0)


python
Author(s):
autogenerated on Sat Jun 8 2019 20:30:20