Merge branch 'hotfix/ros_odometry'

docs/Doxyfile

@@ -38,7 +38,7 @@ PROJECT_NAME           = "qiBullet"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 1.4.4
+PROJECT_NUMBER         = 1.4.5
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

qibullet/__init__.py

@@ -11,4 +11,4 @@
 from qibullet.ros_wrapper import PepperRosWrapper
 
 name = 'qibullet'
-__version__ = "1.4.4"
+__version__ = "1.4.5"

qibullet/ros_wrapper.py

@@ -178,38 +178,49 @@ def _broadcastOdometry(self, odometry_publisher):
         Parameters:
             odometry_publisher - The ROS publisher for the odometry message
         """
-        # Send Transform odom
-        x, y, theta = self.robot.getPosition()
+        # Setup the odom transform
+        if isinstance(self.robot, PepperVirtual) and not OFFICIAL_DRIVER:
+            reference_link = "base_footprint"
+        else:
+            reference_link = "torso"
+
+        # TODO: different models are used between qibullet and the ros stack,
+        # the base footprint will be slighlty underground when the robot is
+        # referenced in odom. Should be corrected, either by modifying the
+        # qibullet model or updating the model used by the ros stack
+        translation, quaternion = self.robot.getLinkPosition(reference_link)
         odom_trans = TransformStamped()
         odom_trans.header.frame_id = "odom"
         odom_trans.child_frame_id = "base_link"
         odom_trans.header.stamp = rospy.get_rostime()
-        odom_trans.transform.translation.x = x
-        odom_trans.transform.translation.y = y
-        odom_trans.transform.translation.z = 0
-        quaternion = pybullet.getQuaternionFromEuler([0, 0, theta])
+
+        odom_trans.transform.translation.x = translation[0]
+        odom_trans.transform.translation.y = translation[1]
+        odom_trans.transform.translation.z = translation[2]
         odom_trans.transform.rotation.x = quaternion[0]
         odom_trans.transform.rotation.y = quaternion[1]
         odom_trans.transform.rotation.z = quaternion[2]
         odom_trans.transform.rotation.w = quaternion[3]
-        self.transform_broadcaster.sendTransform(odom_trans)
-        # Set up the odometry
+
+        # Setup the odometry
         odom = Odometry()
         odom.header.stamp = rospy.get_rostime()
         odom.header.frame_id = "odom"
-        odom.pose.pose.position.x = x
-        odom.pose.pose.position.y = y
+        odom.pose.pose.position.x = translation[0]
+        odom.pose.pose.position.y = translation[1]
         odom.pose.pose.position.z = 0.0
         odom.pose.pose.orientation = odom_trans.transform.rotation
         odom.child_frame_id = "base_link"
         [vx, vy, vz], [wx, wy, wz] = pybullet.getBaseVelocity(
             self.robot.getRobotModel(),
-            self.robot.getPhysicsClientId(),
             physicsClientId=self.robot.getPhysicsClientId())
 
         odom.twist.twist.linear.x = vx
         odom.twist.twist.linear.y = vy
         odom.twist.twist.angular.z = wz
+
+        # Send the odom transform and the odometry
+        self.transform_broadcaster.sendTransform(odom_trans)
         odometry_publisher.publish(odom)
 
     def _broadcastCamera(self, camera, image_publisher, info_publisher):
@@ -686,7 +697,7 @@ def _broadcastLasers(self, laser_publisher):
             corresponding to the laser info of the pepper robot (for API
             consistency)
         """
-        if not self.robot.laser_manager.isActive():
+        if not self.robot.laser_manager.isAlive():
             return
 
         scan = LaserScan()

setup.py

@@ -86,7 +86,7 @@ def run(self):
 
 setuptools.setup(
     name="qibullet",
-    version="1.4.4",
+    version="1.4.5",
     author="Maxime Busy, Maxime Caniot",
     author_email="",
     description="Bullet-based simulation for SoftBank Robotics' robots",