From 1df4bc46a707373b96861938965de162679a7785 Mon Sep 17 00:00:00 2001 From: Jasper Date: Wed, 15 Nov 2023 14:06:22 +0100 Subject: [PATCH 1/7] add feet positions to model states published by wolfgang webots interface --- .../webots_supervisor_controller.py | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py index 2eb37f28d..ae421690e 100644 --- a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py +++ b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py @@ -340,6 +340,29 @@ def publish_model_states(self): msg.name.append(robot_name + "_head") msg.pose.append(head_pose) + l_foot_node = robot_node.getFromProtoDef("l_foot") + l_foot_position = l_foot_node.getPosition() + l_foot_orientation = l_foot_node.getOrientation() + l_foot_orientation_quat = transforms3d.quaternions.mat2quat(np.reshape(l_foot_orientation, (3, 3))) + l_foot_pose = Pose() + l_foot_pose.position = Point(x=l_foot_position[0], y=l_foot_position[1], z=l_foot_position[2]) + l_foot_pose.orientation = Quaternion(x=l_foot_orientation_quat[1], y=l_foot_orientation_quat[2], + z=l_foot_orientation_quat[3], w=l_foot_orientation_quat[0]) + msg.name.append(robot_name + "_l_foot") + msg.pose.append(l_foot_pose) + + r_foot_node = robot_node.getFromProtoDef("r_foot") + r_foot_position = r_foot_node.getPosition() + r_foot_orientation = r_foot_node.getOrientation() + r_foot_orientation_quat = transforms3d.quaternions.mat2quat(np.reshape(r_foot_orientation, (3, 3))) + r_foot_pose = Pose() + r_foot_pose.position = Point(x=r_foot_position[0], y=r_foot_position[1], z=r_foot_position[2]) + r_foot_pose.orientation = Quaternion(x=r_foot_orientation_quat[1], y=r_foot_orientation_quat[2], + z=r_foot_orientation_quat[3], w=r_foot_orientation_quat[0]) + msg.name.append(robot_name + "_r_foot") + msg.pose.append(r_foot_pose) + + if self.ball is not None: ball_position = self.ball.getField("translation").getSFVec3f() ball_pose = Pose() From ac8e9f6c0cf32ef5575b90455608009ca579308e Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Fri, 24 Nov 2023 14:45:12 +0100 Subject: [PATCH 2/7] switch msgs --- .../webots_supervisor_controller.py | 38 +++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py index 7220da04f..bb433bb46 100644 --- a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py +++ b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py @@ -2,9 +2,10 @@ from rclpy.node import Node as RclpyNode from geometry_msgs.msg import Quaternion, Pose, Point, Twist -from gazebo_msgs.msg import ModelStates + from bitbots_msgs.srv import SetObjectPose, SetObjectPosition from rclpy.time import Time +from bitbots_msgs.msg import SimInfo from rosgraph_msgs.msg import Clock from std_srvs.srv import Empty @@ -90,7 +91,7 @@ def __init__(self, ros_node: Node = None, ros_active=False, mode='normal', base_ clock_topic = base_ns + "clock" model_topic = base_ns + "model_states" self.clock_publisher = self.ros_node.create_publisher(Clock, clock_topic, 1) - self.model_state_publisher = self.ros_node.create_publisher(ModelStates, model_topic, 1) + self.model_state_publisher = self.ros_node.create_publisher(SimInfo, model_topic, 1) self.reset_service = self.ros_node.create_service(Empty, base_ns + "reset", self.reset) self.reset_pose_service = self.ros_node.create_service(Empty, base_ns + "reset_pose", self.set_initial_poses) self.set_robot_pose_service = self.ros_node.create_service(SetObjectPose, base_ns + "set_robot_pose", @@ -309,15 +310,15 @@ def get_link_velocities(self, link, name="amy"): def publish_model_states(self): if self.model_state_publisher.get_subscription_count() != 0: - msg = ModelStates() + msg = SimInfo() for robot_name, robot_node in self.robot_nodes.items(): position, orientation = self.get_robot_pose_quat(name=robot_name) robot_pose = Pose() robot_pose.position = Point(x=position[0], y=position[1], z=position[2]) robot_pose.orientation = Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3]) - msg.name.append(robot_name) - msg.pose.append(robot_pose) + msg.base_link_pose = robot_pose + lin_vel, ang_vel = self.get_robot_velocity(robot_name) twist = Twist() twist.linear.x = lin_vel[0] @@ -326,7 +327,7 @@ def publish_model_states(self): twist.angular.x = ang_vel[0] twist.angular.y = ang_vel[1] twist.angular.z = ang_vel[2] - msg.twist.append(twist) + msg.base_link_twist = twist head_node = robot_node.getFromProtoDef("head") head_position = head_node.getPosition() @@ -336,8 +337,9 @@ def publish_model_states(self): head_pose.position = Point(x=head_position[0], y=head_position[1], z=head_position[2]) head_pose.orientation = Quaternion(x=head_orientation_quat[1], y=head_orientation_quat[2], z=head_orientation_quat[3], w=head_orientation_quat[0]) - msg.name.append(robot_name + "_head") - msg.pose.append(head_pose) + # I currently do not need the head + # msg.name.append(robot_name + "_head") + # msg.pose.append(head_pose) l_foot_node = robot_node.getFromProtoDef("l_foot") l_foot_position = l_foot_node.getPosition() @@ -347,8 +349,7 @@ def publish_model_states(self): l_foot_pose.position = Point(x=l_foot_position[0], y=l_foot_position[1], z=l_foot_position[2]) l_foot_pose.orientation = Quaternion(x=l_foot_orientation_quat[1], y=l_foot_orientation_quat[2], z=l_foot_orientation_quat[3], w=l_foot_orientation_quat[0]) - msg.name.append(robot_name + "_l_foot") - msg.pose.append(l_foot_pose) + msg.l_foot_pose = l_foot_pose r_foot_node = robot_node.getFromProtoDef("r_foot") r_foot_position = r_foot_node.getPosition() @@ -358,16 +359,15 @@ def publish_model_states(self): r_foot_pose.position = Point(x=r_foot_position[0], y=r_foot_position[1], z=r_foot_position[2]) r_foot_pose.orientation = Quaternion(x=r_foot_orientation_quat[1], y=r_foot_orientation_quat[2], z=r_foot_orientation_quat[3], w=r_foot_orientation_quat[0]) - msg.name.append(robot_name + "_r_foot") - msg.pose.append(r_foot_pose) + msg.r_foot_pose = r_foot_pose - if self.ball is not None: - ball_position = self.ball.getField("translation").getSFVec3f() - ball_pose = Pose() - ball_pose.position = Point(x=ball_position[0], y=ball_position[1], z=ball_position[2]) - ball_pose.orientation = Quaternion() - msg.name.append("ball") - msg.pose.append(ball_pose) + # if self.ball is not None: + # ball_position = self.ball.getField("translation").getSFVec3f() + # ball_pose = Pose() + # ball_pose.position = Point(x=ball_position[0], y=ball_position[1], z=ball_position[2]) + # ball_pose.orientation = Quaternion() + # msg.name.append("ball") + # msg.pose.append(ball_pose) self.model_state_publisher.publish(msg) From 0b9fd9fdc6df1a389b147eb6d32f7515a72dd258 Mon Sep 17 00:00:00 2001 From: Jasper Date: Mon, 23 Oct 2023 11:05:06 +0200 Subject: [PATCH 3/7] add feet imu to model and robot controller --- .../launch/imu_filter_sim.launch | 5 +- wolfgang_webots_sim/launch/simulation.launch | 11 ++ .../protos/robots/Wolfgang/Wolfgang.proto | 124 ++++++++++++++++++ .../webots_robot_controller.py | 72 +++++++--- 4 files changed, 193 insertions(+), 19 deletions(-) diff --git a/wolfgang_webots_sim/launch/imu_filter_sim.launch b/wolfgang_webots_sim/launch/imu_filter_sim.launch index 01a2639e3..557387239 100644 --- a/wolfgang_webots_sim/launch/imu_filter_sim.launch +++ b/wolfgang_webots_sim/launch/imu_filter_sim.launch @@ -4,13 +4,16 @@ + + name="complementary_filter_gain_node_$(var ns)" output="screen"> + + diff --git a/wolfgang_webots_sim/launch/simulation.launch b/wolfgang_webots_sim/launch/simulation.launch index b9511ea3f..972b24535 100644 --- a/wolfgang_webots_sim/launch/simulation.launch +++ b/wolfgang_webots_sim/launch/simulation.launch @@ -34,6 +34,17 @@ + + + + + + + + + + + diff --git a/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto b/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto index 53ad73c84..077209048 100644 --- a/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto +++ b/wolfgang_webots_sim/protos/robots/Wolfgang/Wolfgang.proto @@ -1906,6 +1906,68 @@ PROTO Wolfgang [ translation -0.069100 -0.000000 -0.026000 rotation 0.572979 0.572976 -0.585998 2.081475 children [ + Accelerometer { + name "imu_r_foot accelerometer" + lookupTable [ + -156.960000 -156.960000 0.000210, + -78.480000 -78.480000 0.000419, + -39.240000 -39.240000 0.000839, + -19.620000 -19.620000 0.001677, + -9.810000 -9.810000 0.003354, + -4.905000 -4.905000 0.006708, + -2.452500 -2.452500 0.013416, + -1.226250 -1.226250 0.026833, + -0.613125 -0.613125 0.053666, + -0.306563 -0.306563 0.107331, + -0.153281 -0.153281 0.214663, + -0.076641 -0.076641 0.429325, + 0 0 0 + 0.076641 0.076641 0.429325, + 0.153281 0.153281 0.214663, + 0.306563 0.306563 0.107331, + 0.613125 0.613125 0.053666, + 1.226250 1.226250 0.026833, + 2.452500 2.452500 0.013416, + 4.905000 4.905000 0.006708, + 9.810000 9.810000 0.003354, + 19.620000 19.620000 0.001677, + 39.240000 39.240000 0.000839, + 78.480000 78.480000 0.000419, + 156.960000 156.960000 0.000210, + ] + resolution 0.00479003906 + } + Gyro { + name "imu_r_foot gyro" + lookupTable [ + -34.906590 -34.906590 0.000031, + -17.453295 -17.453295 0.000061, + -8.726648 -8.726648 0.000122, + -4.363324 -4.363324 0.000244, + -2.181662 -2.181662 0.000488, + -1.090831 -1.090831 0.000977, + -0.545415 -0.545415 0.001953, + -0.272708 -0.272708 0.003906, + -0.136354 -0.136354 0.007812, + -0.068177 -0.068177 0.015625, + -0.034088 -0.034088 0.031250, + -0.017044 -0.017044 0.062500, + 0 0 0 + 0.017044 0.017044 0.062500, + 0.034088 0.034088 0.031250, + 0.068177 0.068177 0.015625, + 0.136354 0.136354 0.007812, + 0.272708 0.272708 0.003906, + 0.545415 0.545415 0.001953, + 1.090831 1.090831 0.000977, + 2.181662 2.181662 0.000488, + 4.363324 4.363324 0.000244, + 8.726648 8.726648 0.000122, + 17.453295 17.453295 0.000061, + 34.906590 34.906590 0.000031, + ] + resolution 0.00106526458 + } Transform { translation -0.030000 0.006500 -0.034000 rotation 0.000000 1.000000 0.000000 0.000000 @@ -3326,6 +3388,68 @@ PROTO Wolfgang [ translation 0.069100 0.000000 -0.026000 rotation 0.577350 -0.577350 0.577352 2.094399 children [ + Accelerometer { + name "imu_l_foot accelerometer" + lookupTable [ + -156.960000 -156.960000 0.000210, + -78.480000 -78.480000 0.000419, + -39.240000 -39.240000 0.000839, + -19.620000 -19.620000 0.001677, + -9.810000 -9.810000 0.003354, + -4.905000 -4.905000 0.006708, + -2.452500 -2.452500 0.013416, + -1.226250 -1.226250 0.026833, + -0.613125 -0.613125 0.053666, + -0.306563 -0.306563 0.107331, + -0.153281 -0.153281 0.214663, + -0.076641 -0.076641 0.429325, + 0 0 0 + 0.076641 0.076641 0.429325, + 0.153281 0.153281 0.214663, + 0.306563 0.306563 0.107331, + 0.613125 0.613125 0.053666, + 1.226250 1.226250 0.026833, + 2.452500 2.452500 0.013416, + 4.905000 4.905000 0.006708, + 9.810000 9.810000 0.003354, + 19.620000 19.620000 0.001677, + 39.240000 39.240000 0.000839, + 78.480000 78.480000 0.000419, + 156.960000 156.960000 0.000210, + ] + resolution 0.00479003906 + } + Gyro { + name "imu_l_foot gyro" + lookupTable [ + -34.906590 -34.906590 0.000031, + -17.453295 -17.453295 0.000061, + -8.726648 -8.726648 0.000122, + -4.363324 -4.363324 0.000244, + -2.181662 -2.181662 0.000488, + -1.090831 -1.090831 0.000977, + -0.545415 -0.545415 0.001953, + -0.272708 -0.272708 0.003906, + -0.136354 -0.136354 0.007812, + -0.068177 -0.068177 0.015625, + -0.034088 -0.034088 0.031250, + -0.017044 -0.017044 0.062500, + 0 0 0 + 0.017044 0.017044 0.062500, + 0.034088 0.034088 0.031250, + 0.068177 0.068177 0.015625, + 0.136354 0.136354 0.007812, + 0.272708 0.272708 0.003906, + 0.545415 0.545415 0.001953, + 1.090831 1.090831 0.000977, + 2.181662 2.181662 0.000488, + 4.363324 4.363324 0.000244, + 8.726648 8.726648 0.000122, + 17.453295 17.453295 0.000061, + 34.906590 34.906590 0.000031, + ] + resolution 0.00106526458 + } Transform { translation -0.060000 0.163500 -0.034000 rotation 0.000000 1.000000 0.000000 0.000000 diff --git a/wolfgang_webots_sim/wolfgang_webots_sim/webots_robot_controller.py b/wolfgang_webots_sim/wolfgang_webots_sim/webots_robot_controller.py index f1d02d425..9ae75dcfa 100644 --- a/wolfgang_webots_sim/wolfgang_webots_sim/webots_robot_controller.py +++ b/wolfgang_webots_sim/wolfgang_webots_sim/webots_robot_controller.py @@ -273,6 +273,16 @@ def __init__(self, ros_node: Node = None, ros_active=False, robot='wolfgang', ro self.accel_head.enable(self.timestep) self.gyro_head = self.robot_node.getDevice("imu_head gyro") self.gyro_head.enable(self.timestep) + + self.accel_l_foot = self.robot_node.getDevice("imu_l_foot accelerometer") + self.accel_l_foot.enable(self.timestep) + self.gyro_l_foot = self.robot_node.getDevice("imu_l_foot gyro") + self.gyro_l_foot.enable(self.timestep) + + self.accel_r_foot = self.robot_node.getDevice("imu_r_foot accelerometer") + self.accel_r_foot.enable(self.timestep) + self.gyro_r_foot = self.robot_node.getDevice("imu_r_foot gyro") + self.gyro_r_foot.enable(self.timestep) self.camera = self.robot_node.getDevice(camera_name) self.camera_counter = 0 if self.camera_active: @@ -299,9 +309,13 @@ def __init__(self, ros_node: Node = None, ros_active=False, robot='wolfgang', ro "camera_optical_frame").get_parameter_value().string_value self.head_imu_frame = self.ros_node.get_parameter("head_imu_frame").get_parameter_value().string_value self.pub_js = self.ros_node.create_publisher(JointState, base_ns + "joint_states", 1) + self.pub_imu = self.ros_node.create_publisher(Imu, base_ns + "imu/data_raw", 1) - self.pub_imu_head = self.ros_node.create_publisher(Imu, base_ns + "imu_head/data", 1) + self.pub_imu_head = self.ros_node.create_publisher(Imu, base_ns + "imu_head/data_raw", 1) + self.pub_imu_l_foot = self.ros_node.create_publisher(Imu, base_ns + "imu_l_foot/data_raw", 1) + self.pub_imu_r_foot = self.ros_node.create_publisher(Imu, base_ns + "imu_r_foot/data_raw", 1) + self.pub_cam = self.ros_node.create_publisher(Image, base_ns + "camera/image_proc", 1) self.pub_cam_info = self.ros_node.create_publisher(CameraInfo, base_ns + "camera/camera_info", 1) @@ -440,22 +454,31 @@ def get_joint_state_msg(self): def publish_joint_states(self): self.pub_js.publish(self.get_joint_state_msg()) - def get_imu_msg(self, head=False): + def get_imu_msg(self, which="torso"): msg = Imu() msg.header.stamp = Time(seconds=int(self.time), nanoseconds=self.time % 1 * 1e9).to_msg() - if head: - msg.header.frame_id = self.head_imu_frame - else: + if which == "torso": msg.header.frame_id = self.imu_frame - - # change order because webots has different axis - if head: + accel_vels = self.accel.getValues() + msg.linear_acceleration.x = accel_vels[0] + msg.linear_acceleration.y = accel_vels[1] + msg.linear_acceleration.z = accel_vels[2] + elif which == "head": + # change order because rotated frames in webots are weired + msg.header.frame_id = self.head_imu_frame accel_vels = self.accel_head.getValues() msg.linear_acceleration.x = accel_vels[2] msg.linear_acceleration.y = -accel_vels[0] msg.linear_acceleration.z = -accel_vels[1] - else: - accel_vels = self.accel.getValues() + elif which == "l_foot": + msg.header.frame_id = self.l_sole_frame + accel_vels = self.accel_l_foot.getValues() + msg.linear_acceleration.x = accel_vels[0] + msg.linear_acceleration.y = accel_vels[1] + msg.linear_acceleration.z = accel_vels[2] + elif which == "r_foot": + msg.header.frame_id = self.r_sole_frame + accel_vels = self.accel_r_foot.getValues() msg.linear_acceleration.x = accel_vels[0] msg.linear_acceleration.y = accel_vels[1] msg.linear_acceleration.z = accel_vels[2] @@ -465,12 +488,7 @@ def get_imu_msg(self, head=False): if msg.linear_acceleration.x == 0 and msg.linear_acceleration.y == 0 and msg.linear_acceleration.z == 0: msg.linear_acceleration.z = 0.001 - if head: - gyro_vels = self.gyro_head.getValues() - msg.angular_velocity.x = gyro_vels[2] - msg.angular_velocity.y = -gyro_vels[0] - msg.angular_velocity.z = -gyro_vels[1] - else: + if which == "torso": gyro_vels = self.gyro.getValues() msg.angular_velocity.x = gyro_vels[0] msg.angular_velocity.y = gyro_vels[1] @@ -478,12 +496,30 @@ def get_imu_msg(self, head=False): msg.angular_velocity.z = gyro_vels[2] else: msg.angular_velocity.z = 0.0 + elif which == "head": + gyro_vels = self.gyro_head.getValues() + msg.angular_velocity.x = gyro_vels[2] + msg.angular_velocity.y = -gyro_vels[0] + msg.angular_velocity.z = -gyro_vels[1] + elif which == "l_foot": + gyro_vels = self.gyro_l_foot.getValues() + msg.angular_velocity.x = gyro_vels[0] + msg.angular_velocity.y = gyro_vels[1] + msg.angular_velocity.z = gyro_vels[2] + elif which == "r_foot": + gyro_vels = self.gyro_r_foot.getValues() + msg.angular_velocity.x = gyro_vels[0] + msg.angular_velocity.y = gyro_vels[1] + msg.angular_velocity.z = gyro_vels[2] return msg def publish_imu(self): - self.pub_imu.publish(self.get_imu_msg(head=False)) + self.pub_imu.publish(self.get_imu_msg()) if self.is_wolfgang: - self.pub_imu_head.publish(self.get_imu_msg(head=True)) + self.pub_imu_head.publish(self.get_imu_msg(which="head")) + self.pub_imu_l_foot.publish(self.get_imu_msg(which="l_foot")) + self.pub_imu_r_foot.publish(self.get_imu_msg(which="r_foot")) + def publish_camera(self): img_msg = Image() From b5de00c79b22c216be3a8e1cdeb2a85c2f05522b Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Thu, 23 Nov 2023 16:44:32 +0100 Subject: [PATCH 4/7] Update GitHub Issue and PR templates --- .github/ISSUE_TEMPLATE/1_bug_report.md | 20 ++++---------------- .github/ISSUE_TEMPLATE/2_feature_request.md | 13 ++++--------- .github/ISSUE_TEMPLATE/3_need_help.md | 10 +++------- .github/pull_request_template.md | 12 +++++++----- 4 files changed, 18 insertions(+), 37 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/1_bug_report.md b/.github/ISSUE_TEMPLATE/1_bug_report.md index fad4c5534..cc12b25a6 100644 --- a/.github/ISSUE_TEMPLATE/1_bug_report.md +++ b/.github/ISSUE_TEMPLATE/1_bug_report.md @@ -1,11 +1,10 @@ --- name: "\U0001F41B Bug report" about: Report a misbehavior or other bug -title: '' labels: 'bug' -assignees: '' --- - +# Summary + ## Expected behavior @@ -15,21 +14,10 @@ assignees: '' ## Steps to Reproduce - + 1. 2. 3. -... - -## Context (Environment) - - -- [ ] RViz -- [ ] Simulator -- [ ] Robot -- [ ] Local ## Possible Solution - - - + diff --git a/.github/ISSUE_TEMPLATE/2_feature_request.md b/.github/ISSUE_TEMPLATE/2_feature_request.md index 4aad9e5d2..875cf49cc 100644 --- a/.github/ISSUE_TEMPLATE/2_feature_request.md +++ b/.github/ISSUE_TEMPLATE/2_feature_request.md @@ -3,14 +3,10 @@ name: "\U0001F680 Feature request" about: Suggest an idea for this project labels: 'enhancement' --- +# Summary + - - -## Is your feature request related to a problem? Please describe. +## Is your feature request related to a problem? ## Describe the solution you'd like @@ -18,5 +14,4 @@ Please fill in as much of the template below as you're able. ## Describe alternatives you've considered - - + diff --git a/.github/ISSUE_TEMPLATE/3_need_help.md b/.github/ISSUE_TEMPLATE/3_need_help.md index 75b65cd13..48cba38a6 100644 --- a/.github/ISSUE_TEMPLATE/3_need_help.md +++ b/.github/ISSUE_TEMPLATE/3_need_help.md @@ -1,14 +1,11 @@ --- -name: "⁉️ Need help?" +name: "⁉️ Help me" about: "Get help with using or improving our software" -title: '' -labels: '' -assignees: '' +labels: 'question' --- - -## What I'm trying to do +# What I'm trying to do ## What I've tried @@ -16,4 +13,3 @@ assignees: '' ## Additional context - diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index ff7abe1fc..41cf5a85f 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1,3 +1,6 @@ +# Summary + + ## Proposed changes @@ -6,12 +9,11 @@ -## Necessary checks -- [ ] Update package version -- [ ] Run `catkin build` +## Checklist + +- [ ] Run `colcon build` - [ ] Write documentation - [ ] Create issues for future work - [ ] Test on your machine - [ ] Test on the robot -- [ ] Put the PR on our Project board - +- [ ] This PR is on our `Software` project board From 24e3ff2328978e2c0d3ab88da8f440fde18ee20b Mon Sep 17 00:00:00 2001 From: Valerie Date: Fri, 1 Dec 2023 12:02:58 +0100 Subject: [PATCH 5/7] add time stamp to msg --- .../wolfgang_webots_sim/webots_supervisor_controller.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py index bb433bb46..9b3d04cde 100644 --- a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py +++ b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py @@ -359,7 +359,9 @@ def publish_model_states(self): r_foot_pose.position = Point(x=r_foot_position[0], y=r_foot_position[1], z=r_foot_position[2]) r_foot_pose.orientation = Quaternion(x=r_foot_orientation_quat[1], y=r_foot_orientation_quat[2], z=r_foot_orientation_quat[3], w=r_foot_orientation_quat[0]) + msg.r_foot_pose = r_foot_pose + msg.header.stamp = self.ros_node.get_clock().now() # if self.ball is not None: From d676d4eb2b29b8c0a7029fb76b60707d06796199 Mon Sep 17 00:00:00 2001 From: Valerie Date: Fri, 1 Dec 2023 12:39:18 +0100 Subject: [PATCH 6/7] fix wrong type of time stamp --- .../wolfgang_webots_sim/webots_supervisor_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py index 9b3d04cde..216c41c16 100644 --- a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py +++ b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py @@ -361,7 +361,7 @@ def publish_model_states(self): z=r_foot_orientation_quat[3], w=r_foot_orientation_quat[0]) msg.r_foot_pose = r_foot_pose - msg.header.stamp = self.ros_node.get_clock().now() + msg.header.stamp = self.ros_node.get_clock().now().to_msg() # if self.ball is not None: From 39c2ab8cd16fcba8e281aafc7e9d149a92486612 Mon Sep 17 00:00:00 2001 From: Valerie Date: Sun, 3 Dec 2023 10:32:32 +0100 Subject: [PATCH 7/7] use sim time instead of ros time --- .../wolfgang_webots_sim/webots_supervisor_controller.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py index 216c41c16..7799bd2d6 100644 --- a/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py +++ b/wolfgang_webots_sim/wolfgang_webots_sim/webots_supervisor_controller.py @@ -361,7 +361,11 @@ def publish_model_states(self): z=r_foot_orientation_quat[3], w=r_foot_orientation_quat[0]) msg.r_foot_pose = r_foot_pose - msg.header.stamp = self.ros_node.get_clock().now().to_msg() + + seconds = int(self.time) + nanosec = int((self.time - seconds) * 1e9) + msg.header.stamp = Time(seconds=seconds, nanoseconds=nanosec).to_msg() + # if self.ball is not None: