From 378507b498c8a0e26dd163bedfa926eeaedbf0b3 Mon Sep 17 00:00:00 2001 From: Maxime Busy Date: Thu, 24 Mar 2022 20:02:27 +0100 Subject: [PATCH 1/4] Update parameters of getBaseVelocity --- qibullet/ros_wrapper.py | 1 - 1 file changed, 1 deletion(-) diff --git a/qibullet/ros_wrapper.py b/qibullet/ros_wrapper.py index 057bde8..e9b2137 100644 --- a/qibullet/ros_wrapper.py +++ b/qibullet/ros_wrapper.py @@ -204,7 +204,6 @@ def _broadcastOdometry(self, odometry_publisher): 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 From e261d386c4bbf330b99dbb57d4652c61004b0463 Mon Sep 17 00:00:00 2001 From: Maxime Busy Date: Thu, 24 Mar 2022 20:09:53 +0100 Subject: [PATCH 2/4] Fix the isAlive vs isActive method for laser broadcast --- qibullet/ros_wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/qibullet/ros_wrapper.py b/qibullet/ros_wrapper.py index e9b2137..0c0c2ca 100644 --- a/qibullet/ros_wrapper.py +++ b/qibullet/ros_wrapper.py @@ -685,7 +685,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() From 21ccc3c429061be1e8772ec695e3ef7dec9e9c58 Mon Sep 17 00:00:00 2001 From: Maxime Busy Date: Fri, 25 Mar 2022 18:04:44 +0100 Subject: [PATCH 3/4] Add modifications to accurately broadcast odom for the official ROS stack --- qibullet/ros_wrapper.py | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/qibullet/ros_wrapper.py b/qibullet/ros_wrapper.py index 0c0c2ca..e42db99 100644 --- a/qibullet/ros_wrapper.py +++ b/qibullet/ros_wrapper.py @@ -178,27 +178,36 @@ 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" @@ -209,6 +218,9 @@ def _broadcastOdometry(self, odometry_publisher): 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): From 645d6d65785170d60d909548da9e860cbc576bdb Mon Sep 17 00:00:00 2001 From: mbusy Date: Mon, 28 Mar 2022 17:35:32 +0200 Subject: [PATCH 4/4] Bump to 1.4.5 --- docs/Doxyfile | 2 +- qibullet/__init__.py | 2 +- setup.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Doxyfile b/docs/Doxyfile index c3a92f8..0e842de 100644 --- a/docs/Doxyfile +++ b/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 diff --git a/qibullet/__init__.py b/qibullet/__init__.py index b600d19..01d37df 100644 --- a/qibullet/__init__.py +++ b/qibullet/__init__.py @@ -11,4 +11,4 @@ from qibullet.ros_wrapper import PepperRosWrapper name = 'qibullet' -__version__ = "1.4.4" +__version__ = "1.4.5" diff --git a/setup.py b/setup.py index ac591db..4494075 100644 --- a/setup.py +++ b/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",