From 754640a6fe36e3ce28d0ad6a9f07f687dd77b0bc Mon Sep 17 00:00:00 2001 From: Stephan Sundermann Date: Tue, 24 Oct 2023 14:44:06 +0200 Subject: [PATCH] Fixes for live launch --- .../src/ArduinoCommunication.cpp | 2 +- .../launch/communication/Communication.launch | 13 +--- .../autominy/launch/sensors/Arduino.launch | 4 +- .../src/autominy/launch/sensors/D435.launch | 22 ++---- .../launch/sensors/Localization.launch | 18 ++--- .../src/autominy/launch/sensors/Map.launch | 2 + .../autominy/launch/sensors/RPLidar.launch | 2 +- .../sensors/RoadMarkingLocalization.launch | 7 +- .../launch/vehicle_model/VehicleModel.launch | 1 + .../car_description/launch/description.launch | 3 +- .../src/LowVoltageShutdownNodelet.cpp | 2 +- .../map_publisher/launch/robotics_lab.launch | 2 +- .../road_marking_localization/CMakeLists.txt | 5 +- .../road_marking_localization/scripts/gps.py | 55 +++++++++++++++ .../scripts/gps_initialpose.py | 69 +++++++++++++------ .../src/RoadMarkingLocalizationNodelet.cpp | 6 +- 16 files changed, 143 insertions(+), 70 deletions(-) create mode 100644 catkin_ws/src/road_marking_localization/scripts/gps.py diff --git a/catkin_ws/src/arduino_communication/src/ArduinoCommunication.cpp b/catkin_ws/src/arduino_communication/src/ArduinoCommunication.cpp index 61362be9..fb1f94c7 100644 --- a/catkin_ws/src/arduino_communication/src/ArduinoCommunication.cpp +++ b/catkin_ws/src/arduino_communication/src/ArduinoCommunication.cpp @@ -115,7 +115,7 @@ void ArduinoCommunication::spin() { RCLCPP_ERROR_THROTTLE(get_logger(), clk, 1000, "Could not connect to arduino %s", exception.what()); } - rclcpp::spin(nh); + rclcpp::spin_some(nh); r.sleep(); } } diff --git a/catkin_ws/src/autominy/launch/communication/Communication.launch b/catkin_ws/src/autominy/launch/communication/Communication.launch index b4f5523e..3be6c691 100644 --- a/catkin_ws/src/autominy/launch/communication/Communication.launch +++ b/catkin_ws/src/autominy/launch/communication/Communication.launch @@ -1,14 +1,7 @@ - - - - - - + + + diff --git a/catkin_ws/src/autominy/launch/sensors/Arduino.launch b/catkin_ws/src/autominy/launch/sensors/Arduino.launch index 0403af9c..493442f0 100644 --- a/catkin_ws/src/autominy/launch/sensors/Arduino.launch +++ b/catkin_ws/src/autominy/launch/sensors/Arduino.launch @@ -2,6 +2,8 @@ + + @@ -11,5 +13,5 @@ - + diff --git a/catkin_ws/src/autominy/launch/sensors/D435.launch b/catkin_ws/src/autominy/launch/sensors/D435.launch index 8c2ad063..f2a1ea99 100644 --- a/catkin_ws/src/autominy/launch/sensors/D435.launch +++ b/catkin_ws/src/autominy/launch/sensors/D435.launch @@ -3,24 +3,16 @@ - + @@ -36,7 +28,7 @@ - + diff --git a/catkin_ws/src/autominy/launch/sensors/Localization.launch b/catkin_ws/src/autominy/launch/sensors/Localization.launch index 6f1e34be..7e76c161 100644 --- a/catkin_ws/src/autominy/launch/sensors/Localization.launch +++ b/catkin_ws/src/autominy/launch/sensors/Localization.launch @@ -14,18 +14,18 @@ - + - - - + + + + + + + + diff --git a/catkin_ws/src/autominy/launch/sensors/Map.launch b/catkin_ws/src/autominy/launch/sensors/Map.launch index 3a97fafe..4fbb697b 100644 --- a/catkin_ws/src/autominy/launch/sensors/Map.launch +++ b/catkin_ws/src/autominy/launch/sensors/Map.launch @@ -2,6 +2,8 @@ + + diff --git a/catkin_ws/src/autominy/launch/sensors/RPLidar.launch b/catkin_ws/src/autominy/launch/sensors/RPLidar.launch index 62919861..762015f3 100644 --- a/catkin_ws/src/autominy/launch/sensors/RPLidar.launch +++ b/catkin_ws/src/autominy/launch/sensors/RPLidar.launch @@ -30,7 +30,7 @@ - + diff --git a/catkin_ws/src/autominy/launch/sensors/RoadMarkingLocalization.launch b/catkin_ws/src/autominy/launch/sensors/RoadMarkingLocalization.launch index e6a7ec8f..27d9cb79 100644 --- a/catkin_ws/src/autominy/launch/sensors/RoadMarkingLocalization.launch +++ b/catkin_ws/src/autominy/launch/sensors/RoadMarkingLocalization.launch @@ -3,7 +3,7 @@ - - \ No newline at end of file + + diff --git a/catkin_ws/src/autominy/launch/vehicle_model/VehicleModel.launch b/catkin_ws/src/autominy/launch/vehicle_model/VehicleModel.launch index a1c958bf..7a792f31 100644 --- a/catkin_ws/src/autominy/launch/vehicle_model/VehicleModel.launch +++ b/catkin_ws/src/autominy/launch/vehicle_model/VehicleModel.launch @@ -8,5 +8,6 @@ + diff --git a/catkin_ws/src/car_description/launch/description.launch b/catkin_ws/src/car_description/launch/description.launch index 87dcacd6..7cb96833 100755 --- a/catkin_ws/src/car_description/launch/description.launch +++ b/catkin_ws/src/car_description/launch/description.launch @@ -4,6 +4,7 @@ + @@ -12,7 +13,7 @@ value="$(command 'xacro \'$(find-pkg-share car_description)/urdf/$(var model).xacro\' tf_prefix:=$(var tf_prefix) car_name:=$(var tf_prefix)model_car agent_prefix:=$(var agent_prefix)')" /> - diff --git a/catkin_ws/src/low_voltage_shutdown/src/LowVoltageShutdownNodelet.cpp b/catkin_ws/src/low_voltage_shutdown/src/LowVoltageShutdownNodelet.cpp index 08400bdf..7f5756a7 100644 --- a/catkin_ws/src/low_voltage_shutdown/src/LowVoltageShutdownNodelet.cpp +++ b/catkin_ws/src/low_voltage_shutdown/src/LowVoltageShutdownNodelet.cpp @@ -4,7 +4,7 @@ namespace low_voltage_shutdown { LowVoltageShutdownNodelet::LowVoltageShutdownNodelet(const rclcpp::NodeOptions &opts) : rclcpp::Node("low_voltage_shutdown", opts) { shutdownVoltage = declare_parameter("shutdown_voltage", 13.0); - voltageSubscriber = create_subscription("voltage", 10, std::bind(&LowVoltageShutdownNodelet::onVoltage, this, std::placeholders::_1)); + voltageSubscriber = create_subscription("/sensors/voltage", 10, std::bind(&LowVoltageShutdownNodelet::onVoltage, this, std::placeholders::_1)); } void LowVoltageShutdownNodelet::onVoltage(const autominy_msgs::msg::Voltage::ConstSharedPtr &msg) { diff --git a/catkin_ws/src/map_publisher/launch/robotics_lab.launch b/catkin_ws/src/map_publisher/launch/robotics_lab.launch index a43e6a40..4f656b89 100644 --- a/catkin_ws/src/map_publisher/launch/robotics_lab.launch +++ b/catkin_ws/src/map_publisher/launch/robotics_lab.launch @@ -1,6 +1,6 @@ - + diff --git a/catkin_ws/src/road_marking_localization/CMakeLists.txt b/catkin_ws/src/road_marking_localization/CMakeLists.txt index aaaaf608..9a023639 100644 --- a/catkin_ws/src/road_marking_localization/CMakeLists.txt +++ b/catkin_ws/src/road_marking_localization/CMakeLists.txt @@ -81,10 +81,13 @@ install( DESTINATION lib/${PROJECT_NAME} ) +install(PROGRAMS scripts/gps.py scripts/gps_initialpose.py + DESTINATION lib/${PROJECT_NAME}) + ## Mark other files for installation (e.g. launch and bag files, etc.) install( DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) -ament_package() \ No newline at end of file +ament_package() diff --git a/catkin_ws/src/road_marking_localization/scripts/gps.py b/catkin_ws/src/road_marking_localization/scripts/gps.py new file mode 100644 index 00000000..e498f1b6 --- /dev/null +++ b/catkin_ws/src/road_marking_localization/scripts/gps.py @@ -0,0 +1,55 @@ +#!/usr/bin/python3 + +import rclpy +import roslibpy + +from rclpy.node import Node +from rclpy.time import Time +from rclpy.executors import MultiThreadedExecutor + +from nav_msgs.msg import Odometry + +class GPSClient(Node): + def __init__(self): + super().__init__('gps_client') + self.id = self.declare_parameter('id', 16) + self.pub = self.create_publisher(Odometry, '/sensors/odometry/gps', 10) + client = roslibpy.Ros(host='192.168.43.2', port=9090) + client.run() + listener = roslibpy.Topic(client, '/communication/gps/' + str(self.get_parameter('id').value), 'nav_msgs/Odometry') + listener.subscribe(self.on_gps) + + def on_gps(self, message): + odom = Odometry() + odom.header.stamp = self.get_clock().now().to_msg(); + odom.header.frame_id = message['header']['frame_id'] + odom.child_frame_id = message['child_frame_id'] + odom.pose.pose.position.x = message['pose']['pose']['position']['x'] + odom.pose.pose.position.y = message['pose']['pose']['position']['y'] + odom.pose.pose.position.z = message['pose']['pose']['position']['z'] + odom.pose.pose.orientation.x = message['pose']['pose']['orientation']['x'] + odom.pose.pose.orientation.y = message['pose']['pose']['orientation']['y'] + odom.pose.pose.orientation.z = message['pose']['pose']['orientation']['z'] + odom.pose.pose.orientation.w = message['pose']['pose']['orientation']['w'] + odom.twist.twist.linear.x = message['twist']['twist']['linear']['x'] + odom.twist.twist.linear.y = message['twist']['twist']['linear']['y'] + odom.twist.twist.linear.z = message['twist']['twist']['linear']['z'] + odom.twist.twist.angular.x = message['twist']['twist']['angular']['x'] + odom.twist.twist.angular.y = message['twist']['twist']['angular']['y'] + odom.twist.twist.angular.z = message['twist']['twist']['angular']['z'] + self.pub.publish(odom) + + print(message) + +def main(args=None): + rclpy.init(args=args) + gps_client = GPSClient() + executor = MultiThreadedExecutor() + executor.add_node(gps_client) + + rclpy.spin(gps_client) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/catkin_ws/src/road_marking_localization/scripts/gps_initialpose.py b/catkin_ws/src/road_marking_localization/scripts/gps_initialpose.py index 92aced7d..2a0a581c 100755 --- a/catkin_ws/src/road_marking_localization/scripts/gps_initialpose.py +++ b/catkin_ws/src/road_marking_localization/scripts/gps_initialpose.py @@ -1,37 +1,56 @@ -#! /usr/bin/python3 +#!/usr/bin/python3 -import rospy +import rclpy import math -from tf.transformations import euler_from_quaternion +import numpy as np + from nav_msgs.msg import Odometry from geometry_msgs.msg import PoseWithCovarianceStamped +from rclpy.node import Node + + +def euler_from_quaternion(x, y, z, w): + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) + roll = np.arctan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (w * y - z * x) + pitch = np.arcsin(sinp) + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = np.arctan2(siny_cosp, cosy_cosp) -class GPSInitialpose: + return roll, pitch, yaw + +class GPSInitialpose(Node): def __init__(self): - self.initialpose_pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=1) - self.gps_sub = rospy.Subscriber("gps", Odometry, self.on_gps, queue_size=1) - self.localization_sub = rospy.Subscriber("localization", Odometry, self.on_localization, queue_size=1) + super().__init__("gps_initialpose") + self.initialpose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1) + self.gps_sub = self.create_subscription(Odometry, "/sensors/odometry/gps", self.on_gps, 1) + self.localization_sub = self.create_subscription(Odometry, "/sensors/localization/filtered_map", self.on_localization, 1) self.localization = None - self.tolerance_xy = rospy.get_param("tolerance_xy", 0.4) - self.tolerance_yaw = rospy.get_param("tolerance_yaw", 1.5) - self.last_correction = rospy.Time.now() + self.declare_parameter("tolerance_xy", 0.4) + self.declare_parameter("tolerance_yaw", 1.5) + self.tolerance_xy = self.get_parameter("tolerance_xy").value + self.tolerance_yaw = self.get_parameter("tolerance_yaw").value + self.last_correction = self.get_clock().now() def on_localization(self, msg): self.localization = msg def on_gps(self, msg): - if self.localization and (rospy.Time.now() - self.last_correction).to_sec() > 3.0: + if self.localization and (self.get_clock().now() - self.last_correction).nanoseconds / 1e9 > 3.0: distance = math.sqrt((self.localization.pose.pose.position.x - msg.pose.pose.position.x) ** 2.0 + ( self.localization.pose.pose.position.y - msg.pose.pose.position.y) ** 2.0) - yaw_localization = euler_from_quaternion((self.localization.pose.pose.orientation.x, - self.localization.pose.pose.orientation.y, - self.localization.pose.pose.orientation.z, - self.localization.pose.pose.orientation.w))[2] + yaw_localization = euler_from_quaternion(self.localization.pose.pose.orientation.x, + self.localization.pose.pose.orientation.y, + self.localization.pose.pose.orientation.z, + self.localization.pose.pose.orientation.w)[2] yaw_gps = euler_from_quaternion( - (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, - msg.pose.pose.orientation.w))[2] + msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w)[2] delta_yaw = math.fabs(yaw_localization - yaw_gps) if distance > self.tolerance_xy: # or delta_yaw > self.tolerance_yaw: @@ -39,12 +58,18 @@ def on_gps(self, msg): pose = PoseWithCovarianceStamped() pose.pose = msg.pose pose.header.frame_id = "map" - pose.header.stamp = rospy.Time.now() + pose.header.stamp = self.get_clock().now().to_msg() self.initialpose_pub.publish(pose) - self.last_correction = rospy.Time.now() + self.last_correction = self.get_clock().now() + +def main(args=None): + rclpy.init(args=args) + + node = GPSInitialpose() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - rospy.init_node('gps_initialpose') - GPSInitialpose() - rospy.spin() + main() diff --git a/catkin_ws/src/road_marking_localization/src/RoadMarkingLocalizationNodelet.cpp b/catkin_ws/src/road_marking_localization/src/RoadMarkingLocalizationNodelet.cpp index 7f7eb7e3..8905cd37 100644 --- a/catkin_ws/src/road_marking_localization/src/RoadMarkingLocalizationNodelet.cpp +++ b/catkin_ws/src/road_marking_localization/src/RoadMarkingLocalizationNodelet.cpp @@ -8,7 +8,7 @@ namespace road_marking_localization { config.y_box = declare_parameter("y_box", 2.0); config.minimum_z = declare_parameter("minimum_z", -0.02); config.maximum_z = declare_parameter("maximum_z", 0.02); - config.threshold = declare_parameter("threshold", 160); + config.threshold = declare_parameter("threshold", 180); config.icp_max_iterations = declare_parameter("icp_max_iterations", 5); config.icp_RANSAC_outlier_rejection_threshold = declare_parameter("icp_RANSAC_outlier_rejection_threshold", 0.0); config.icp_RANSAC_iterations = declare_parameter("icp_RANSAC_iterations", 0); @@ -59,7 +59,7 @@ namespace road_marking_localization { robotLocalizationSetPose = create_client("/sensors/set_pose"); mapSubscriber = create_subscription("map", qos, std::bind(&RoadMarkingLocalizationNodelet::onMap, this, std::placeholders::_1)); - positionEstimateSubscriber = create_subscription("initialpose", 1, std::bind(&RoadMarkingLocalizationNodelet::onEstimatedPosition, this, std::placeholders::_1)); + positionEstimateSubscriber = create_subscription("/initialpose", 1, std::bind(&RoadMarkingLocalizationNodelet::onEstimatedPosition, this, std::placeholders::_1)); auto qos2 = rclcpp::QoS(2).get_rmw_qos_profile(); infraImageSubscriber.subscribe(this, "camera/infra1/image_rect_raw", "raw", qos2); @@ -449,4 +449,4 @@ namespace road_marking_localization { } } -} \ No newline at end of file +}