From f51400a4566ccfcd238d78950508dbdbe524cfa6 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Mon, 2 Dec 2024 20:59:39 +0100 Subject: [PATCH 1/4] Adding noisy odom as hal_interface --- .../hal_interfaces/general/noise_odometry.py | 200 ++++++++++++++++++ 1 file changed, 200 insertions(+) create mode 100644 common/hal_interfaces/hal_interfaces/general/noise_odometry.py diff --git a/common/hal_interfaces/hal_interfaces/general/noise_odometry.py b/common/hal_interfaces/hal_interfaces/general/noise_odometry.py new file mode 100644 index 000000000..8d3bb8e8a --- /dev/null +++ b/common/hal_interfaces/hal_interfaces/general/noise_odometry.py @@ -0,0 +1,200 @@ +import numpy as np +from rclpy.node import Node +from math import asin, atan2, pi +import nav_msgs.msg + + +### AUXILIARY FUNCTIONS ### +class Pose3d: + + def __init__(self): + + self.x = 0 # X coord [meters] + self.y = 0 # Y coord [meters] + self.z = 0 # Z coord [meters] + self.h = 1 # H param + self.yaw = 0 # Yaw angle[rads] + self.pitch = 0 # Pitch angle[rads] + self.roll = 0 # Roll angle[rads] + self.q = [0, 0, 0, 0] # Quaternion + self.timeStamp = 0 # Time stamp [s] + + def __str__(self): + s = "Pose3D: {\n x: " + str(self.x) + "\n y: " + str(self.y) + s = s + "\n z: " + str(self.z) + "\n H: " + str(self.h) + s = ( + s + + "\n Yaw: " + + str(self.yaw) + + "\n Pitch: " + + str(self.pitch) + + "\n Roll: " + + str(self.roll) + ) + s = ( + s + + "\n quaternion: " + + str(self.q) + + "\n timeStamp: " + + str(self.timeStamp) + + "\n}" + ) + return s + + +def quat2Yaw(qw, qx, qy, qz): + """ + Translates from Quaternion to Yaw. + @param qw,qx,qy,qz: Quaternion values + @type qw,qx,qy,qz: float + @return Yaw value translated from Quaternion + """ + + rotateZa0 = 2.0 * (qx * qy + qw * qz) + rotateZa1 = qw * qw + qx * qx - qy * qy - qz * qz + rotateZ = 0.0 + if rotateZa0 != 0.0 and rotateZa1 != 0.0: + rotateZ = atan2(rotateZa0, rotateZa1) + + return rotateZ + + +def quat2Pitch(qw, qx, qy, qz): + """ + Translates from Quaternion to Pitch. + @param qw,qx,qy,qz: Quaternion values + @type qw,qx,qy,qz: float + @return Pitch value translated from Quaternion + """ + + rotateYa0 = -2.0 * (qx * qz - qw * qy) + rotateY = 0.0 + if rotateYa0 >= 1.0: + rotateY = pi / 2.0 + elif rotateYa0 <= -1.0: + rotateY = -pi / 2.0 + else: + rotateY = asin(rotateYa0) + + return rotateY + + +def quat2Roll(qw, qx, qy, qz): + """ + Translates from Quaternion to Roll. + @param qw,qx,qy,qz: Quaternion values + @type qw,qx,qy,qz: float + @return Roll value translated from Quaternion + """ + rotateXa0 = 2.0 * (qy * qz + qw * qx) + rotateXa1 = qw * qw - qx * qx - qy * qy + qz * qz + rotateX = 0.0 + + if rotateXa0 != 0.0 and rotateXa1 != 0.0: + rotateX = atan2(rotateXa0, rotateXa1) + return rotateX + +def euler2quat(yaw, pitch, roll): + + qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) + qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2) + + return [qx, qy, qz, qw] + + +def odometry2Pose3D(odom): + """ + Translates from ROS Odometry to JderobotTypes Pose3d. + @param odom: ROS Odometry to translate + @type odom: Odometry + @return a Pose3d translated from odom + + """ + pose = Pose3d() + ori = odom.pose.pose.orientation + + pose.x = odom.pose.pose.position.x + pose.y = odom.pose.pose.position.y + pose.z = odom.pose.pose.position.z + # pose.h = odom.pose.pose.position.h + pose.yaw = quat2Yaw(ori.w, ori.x, ori.y, ori.z) + pose.pitch = quat2Pitch(ori.w, ori.x, ori.y, ori.z) + pose.roll = quat2Roll(ori.w, ori.x, ori.y, ori.z) + pose.q = [ori.w, ori.x, ori.y, ori.z] + pose.timeStamp = odom.header.stamp.sec + (odom.header.stamp.nanosec * 1e-9) + + return pose + +def gaussian_noise(x,mu=0.0,std=0.1): + noise = np.random.normal(mu, std) * 0.1 + x_noisy = x + noise + return x_noisy + +def add_noise(last_pose, new_pose, base_odom): + + # First odom is real + if (last_pose == None): + return new_pose + + # Next odom is movement from last pose to new pose + noise + base odom + mov_x = new_pose.pose.pose.position.x - last_pose.pose.pose.position.x + mov_y = new_pose.pose.pose.position.y - last_pose.pose.pose.position.y + mov_z = new_pose.pose.pose.position.z - last_pose.pose.pose.position.z + + new_ori = new_pose.pose.pose.orientation + old_ori = last_pose.pose.pose.orientation + + mov_yaw = quat2Yaw(new_ori.w, new_ori.x, new_ori.y, new_ori.z) - quat2Yaw(old_ori.w, old_ori.x, old_ori.y, old_ori.z) + mov_pitch = quat2Pitch(new_ori.w, new_ori.x, new_ori.y, new_ori.z) - quat2Pitch(old_ori.w, old_ori.x, old_ori.y, old_ori.z) + mov_roll = quat2Roll(new_ori.w, new_ori.x, new_ori.y, new_ori.z) - quat2Roll(old_ori.w, old_ori.x, old_ori.y, old_ori.z) + + # Add noise + mov_x = gaussian_noise(mov_x) + mov_y = gaussian_noise(mov_y) + mov_yaw = gaussian_noise(mov_yaw) + + # Get new odom angle + ori = base_odom.pose.pose.orientation + + new_yaw = quat2Yaw(ori.w, ori.x, ori.y, ori.z) + mov_yaw + new_pitch = quat2Pitch(ori.w, ori.x, ori.y, ori.z) + mov_pitch + new_roll = quat2Roll(ori.w, ori.x, ori.y, ori.z) + mov_roll + new_ori = euler2quat(new_yaw, new_pitch, new_roll) + + # Generate new odom + new_odom = nav_msgs.msg.Odometry() + + new_odom.pose.pose.position.x = base_odom.pose.pose.position.x + mov_x + new_odom.pose.pose.position.y = base_odom.pose.pose.position.y + mov_y + new_odom.pose.pose.position.z = base_odom.pose.pose.position.z + mov_z + new_odom.pose.pose.orientation.x = new_ori[0] + new_odom.pose.pose.orientation.y = new_ori[1] + new_odom.pose.pose.orientation.z = new_ori[2] + new_odom.pose.pose.orientation.w = new_ori[3] + new_odom.header.stamp.sec = new_pose.header.stamp.sec + new_odom.header.stamp.nanosec = new_pose.header.stamp.nanosec + + return new_odom + +### HAL INTERFACE ### +class NoisyOdometryNode(Node): + + def __init__(self, topic): + super().__init__("noisy_odometry_node") + self.sub = self.create_subscription( + nav_msgs.msg.Odometry, topic, self.listener_callback, 10 + ) + self.last_pose_ = None + self.noisy_pose = nav_msgs.msg.Odometry() + + def listener_callback(self, msg): + # First odom is real + # Second odom is movement from first real to second real + noise + first odom + # Third odom is movement from second real to third real + noise + second odom + self.noisy_pose = add_noise(self.last_pose_, msg, self.noisy_pose) + self.last_pose_ = msg + + def getPose3d(self): + return odometry2Pose3D(self.noisy_pose) From 64244d16fa10b8c0815db586d8a068d3cbcc4043 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Mon, 2 Dec 2024 21:00:01 +0100 Subject: [PATCH 2/4] Using odom in montecarlo laser loc --- .../montecarlo_laser_loc/python_template/ros2_humble/HAL.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/exercises/static/exercises/montecarlo_laser_loc/python_template/ros2_humble/HAL.py b/exercises/static/exercises/montecarlo_laser_loc/python_template/ros2_humble/HAL.py index 57ba63111..eea9474fc 100755 --- a/exercises/static/exercises/montecarlo_laser_loc/python_template/ros2_humble/HAL.py +++ b/exercises/static/exercises/montecarlo_laser_loc/python_template/ros2_humble/HAL.py @@ -4,6 +4,7 @@ from hal_interfaces.general.motors import MotorsNode from hal_interfaces.general.odometry import OdometryNode +from hal_interfaces.general.noise_odometry import NoisyOdometryNode from hal_interfaces.general.laser import LaserNode from hal_interfaces.general.bumper import BumperNode @@ -20,6 +21,7 @@ ### HAL INIT ### motor_node = MotorsNode("/cmd_vel", 4, 0.3) odometry_node = OdometryNode("/odom") + noisy_odometry_node = NoisyOdometryNode("/odom") laser_node = LaserNode("/roombaROS/laser/scan") bumper_node = BumperNode( [ @@ -32,6 +34,7 @@ # Spin nodes so that subscription callbacks load topic data executor = rclpy.executors.MultiThreadedExecutor() executor.add_node(odometry_node) + executor.add_node(noisy_odometry_node) executor.add_node(laser_node) def __auto_spin() -> None: while rclpy.ok(): @@ -44,6 +47,9 @@ def __auto_spin() -> None: def getPose3d(): return odometry_node.getPose3d() +def getOdom(): + return noisy_odometry_node.getPose3d() + # Bumper def getBumperData(): try: From a0ab4a7514450561fb5050de745d0fd2044184e1 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Mon, 2 Dec 2024 21:02:27 +0100 Subject: [PATCH 3/4] Adding noise levels --- .../hal_interfaces/general/noise_odometry.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/common/hal_interfaces/hal_interfaces/general/noise_odometry.py b/common/hal_interfaces/hal_interfaces/general/noise_odometry.py index 8d3bb8e8a..fca0f3c2d 100644 --- a/common/hal_interfaces/hal_interfaces/general/noise_odometry.py +++ b/common/hal_interfaces/hal_interfaces/general/noise_odometry.py @@ -3,7 +3,6 @@ from math import asin, atan2, pi import nav_msgs.msg - ### AUXILIARY FUNCTIONS ### class Pose3d: @@ -127,12 +126,12 @@ def odometry2Pose3D(odom): return pose -def gaussian_noise(x,mu=0.0,std=0.1): - noise = np.random.normal(mu, std) * 0.1 +def gaussian_noise(x,mu=0.0,std=0.1, noise_level = 0.01): + noise = np.random.normal(mu, std) * 0.01 x_noisy = x + noise return x_noisy -def add_noise(last_pose, new_pose, base_odom): +def add_noise(last_pose, new_pose, base_odom, noise_level): # First odom is real if (last_pose == None): @@ -151,9 +150,9 @@ def add_noise(last_pose, new_pose, base_odom): mov_roll = quat2Roll(new_ori.w, new_ori.x, new_ori.y, new_ori.z) - quat2Roll(old_ori.w, old_ori.x, old_ori.y, old_ori.z) # Add noise - mov_x = gaussian_noise(mov_x) - mov_y = gaussian_noise(mov_y) - mov_yaw = gaussian_noise(mov_yaw) + mov_x = gaussian_noise(mov_x, noise_level = noise_level) + mov_y = gaussian_noise(mov_y, noise_level = noise_level) + mov_yaw = gaussian_noise(mov_yaw, noise_level = noise_level) # Get new odom angle ori = base_odom.pose.pose.orientation @@ -189,11 +188,14 @@ def __init__(self, topic): self.last_pose_ = None self.noisy_pose = nav_msgs.msg.Odometry() + ### Control the amount of noise ### + self.noise_level = 0.01 # 0.1 = a lot + def listener_callback(self, msg): # First odom is real # Second odom is movement from first real to second real + noise + first odom # Third odom is movement from second real to third real + noise + second odom - self.noisy_pose = add_noise(self.last_pose_, msg, self.noisy_pose) + self.noisy_pose = add_noise(self.last_pose_, msg, self.noisy_pose, self.noise_level) self.last_pose_ = msg def getPose3d(self): From 8ded2e0d07244f830cefd4f655390ffe217683c2 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Mon, 2 Dec 2024 21:04:40 +0100 Subject: [PATCH 4/4] Adding autocomplete --- .../autocomplete-snippets/hal_gui_snippets.js | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/react_frontend/src/components/editors/monaco-editor/autocomplete-snippets/hal_gui_snippets.js b/react_frontend/src/components/editors/monaco-editor/autocomplete-snippets/hal_gui_snippets.js index b7e334963..9f9ceec69 100644 --- a/react_frontend/src/components/editors/monaco-editor/autocomplete-snippets/hal_gui_snippets.js +++ b/react_frontend/src/components/editors/monaco-editor/autocomplete-snippets/hal_gui_snippets.js @@ -702,6 +702,13 @@ export const guiAndHalAutoCompleteObj = { descriptions: "Get the orientation of the robot with regarding the map.", }, + { + type: "method", + label: "getOdom()", + code: "getOdom()", + descriptions: + "Get the orientation of the robot with noise regarding the map.", + }, { type: "method", label: "getLaserData()",