diff --git a/src/rktl_control/rktl_control/particle_odom_filter.py b/src/rktl_control/rktl_control/particle_odom_filter.py index 7b1ffd15..2b7282f8 100644 --- a/src/rktl_control/rktl_control/particle_odom_filter.py +++ b/src/rktl_control/rktl_control/particle_odom_filter.py @@ -17,7 +17,7 @@ from rktl_dependencies.transformations import euler_from_quaternion, quaternion_from_euler import numpy as np -from pfilter import ParticleFilter +from rktl_dependencies.pfilter import ParticleFilter from collections import deque from threading import Lock from math import sin, cos, tan, atan, sqrt, pi, exp diff --git a/src/rktl_perception/nodes/projector b/src/rktl_perception/nodes/projector index 1a290a4d..9cead02a 100755 --- a/src/rktl_perception/nodes/projector +++ b/src/rktl_perception/nodes/projector @@ -10,7 +10,7 @@ License: import rclpy from sensor_msgs.msg import Image, CameraInfo from tf2_ros import Buffer, TransformListener -from tf.transformations import translation_matrix, quaternion_matrix +from rktl_dependencies.transformations import translation_matrix, quaternion_matrix import numpy as np diff --git a/src/rktl_sim/rktl_sim/nodes/simulator b/src/rktl_sim/rktl_sim/nodes/simulator index d8ac8621..eca0d8b0 100755 --- a/src/rktl_sim/rktl_sim/nodes/simulator +++ b/src/rktl_sim/rktl_sim/nodes/simulator @@ -13,13 +13,10 @@ import sys from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry import os -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator -import rclpy -======== import time #import rospy ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + from std_srvs.srv import Empty, EmptyResponse from threading import Lock from enum import Enum @@ -64,104 +61,27 @@ class Simulator(object): self.car_properties = None # setting config parameters (stay constant for the whole simulator run) -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - rclpy.init(args=sys.argv) - node = rclpy.create_node('simulator') - - #rclpy.init_node('simulator') - -======== # rospy.init_node('simulator') rclpy.init(args=sys.argv) global node node = rclpy.create_node("simulator") ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + mode = self.get_sim_param('~mode') if mode == 'ideal': self.mode = SimulatorMode.IDEAL elif mode == 'realistic': self.mode = SimulatorMode.REALISTIC else: -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - rclpy.shutdown('unknown sim mode set "{}"'.format(mode)) -======== #rospy.signal_shutdown('unknown sim mode set "{}"'.format(mode)) self.destroy_node('unknown sim mode set "{}"'.format(mode)) ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + self.cmd_lock = Lock() self.reset_lock = Lock() self.car_cmd = (0.0, 0.0) render_enabled = self.get_sim_param('~render', secondParam=False) -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator -======== - #rate = rospy.Rate(self.get_sim_param('~rate', secondParam=30)) ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator - rate = node.create_rate(self.get_sim_param('~rate', secondParam=30)) - self.frame_id = self.get_sim_param('~frame_id', secondParam='map') - self.timeout = self.get_sim_param('~timeout', secondParam=10) - - # setup urdf file paths: a universal way to describe kinematics and dynamics of robots - self.urdf_paths = self.get_sim_param('~urdf') - for path in self.urdf_paths.values(): - self.check_urdf(path) - - self.props = { - 'engine': self.get_sim_param('~engine', secondParam=None), - 'dynamics': self.get_sim_param('~dynamics', secondParam=None), - } - - # prep the simulator for a new run, setting all instance parameters for the sim - - self.spawn_bounds, self.field_setup = self.configure_field() - self.car_ids = {} - self.car_pose_pubs = {} - self.car_odom_pubs = {} - self.car_effort_subs = {} - self.car_cmd_subs = {} - - # TODO: find a better way to not have duplicate code segment - self.car_properties = {'length': self.get_sim_param('/cars/length'), - 'max_speed': self.get_sim_param("/cars/throttle/max_speed"), - 'steering_throw': self.get_sim_param("/cars/steering/max_throw"), - 'throttle_tau': self.get_sim_param("/cars/throttle/tau"), - 'steering_rate': self.get_sim_param("/cars/steering/rate"), - 'simulate_effort': (self.mode == SimulatorMode.REALISTIC)} - - self.sim = simulator.Sim( - self.props, self.urdf_paths, self.spawn_bounds, self.field_setup, render_enabled) - - self.sim.create_ball('ball', init_pose=self.ball_init_pose, - init_speed=self.ball_init_speed, noise=self.ball_noise) - - self.update_all_cars() - - self.cmd_lock = Lock() - self.reset_lock = Lock() - self.last_time = None - - self.reset_cb(None) # janky reset call with mandatory none parameter - - # Publishers -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - self.status_pub = node.create_publisher( - MatchStatus, 'match_status', queue_size=1) - self.ball_pose_pub, self.ball_odom_pub = None, None - if self.mode == SimulatorMode.REALISTIC: - self.ball_pose_pub = rclpy.node.create_publisher(PoseWithCovarianceStamped, - '/ball/pose_sync_early', queue_size=1) - self.ball_odom_pub = rclpy.node.create_publisher(Odometry, - '/ball/odom_truth', queue_size=1) - elif self.mode == SimulatorMode.IDEAL: - self.ball_odom_pub = rclpy.node.create_publisher(Odometry, - '/ball/odom', queue_size=1) - - # Services - srv = rclpy.node.create_service('sim_reset_car_and_ball', Empty, self.reset_cb) - srv2 = rclpy.node.create_service('sim_reset_ball', Empty, self.reset_ball_cb) -======== #self.status_pub = rospy.Publisher('match_status', MatchStatus, queue_size=1) self.status_pub = node.create_publisher(MatchStatus, 'match_status', 1) self.ball_pose_pub, self.ball_odom_pub = None, None @@ -179,36 +99,20 @@ class Simulator(object): # rospy.Service('sim_reset_ball', Empty, self.reset_ball_cb) node.create_service(Empty, 'sim_reset_car_and_ball', self.reset_cb) node.create_service(Empty, 'sim_reset_ball', self.reset_ball_cb) ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + while not rclpy.ok(): self.loop_once() try: rate.sleep() -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - -======== except rclpy.ROSInterruptException: pass ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + def check_urdf(self, urdf_path): """Validates that a URDF exists, then returns its file.""" if urdf_path is None: -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - rclpy.shutdown( - 'no urdf path set for "{}"'.format(urdf_path)) - - i = 0 - while not os.path.isfile(urdf_path) and i < 5: - rclpy.sleep(0.1) # wait for xacro build - i += 1 - - if not os.path.isfile(urdf_path): - rclpy.shutdown( - 'no urdf file exists at path {}'.format(urdf_path)) -======== #rospy.signal_shutdown('no urdf path set for "{}"'.format(urdf_path)) self.destroy_node('no urdf path set for "{}"'.format(urdf_path)) try: @@ -225,7 +129,7 @@ class Simulator(object): if not os.path.isfile(urdf_path): #rospy.signal_shutdown('no urdf file exists at path {}'.format(urdf_path)) self.destroy_node('no urdf file exists at path {}'.format(urdf_path)) ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + def effort_cb(self, effort_msg, car_id): """Sets a car's effort with throttle and steering (overwrites commands).""" @@ -284,14 +188,10 @@ class Simulator(object): def loop_once(self): """Step the simulation once step, updating match status, moving and publishing new car and ball positions.""" self.reset_lock.acquire() -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - - now = self.node.get_clock().now() -======== #now = rospy.Time.now() now = self.get_clock().now() ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + if self.last_time is not None and self.last_time != now: delta_t = (now - self.last_time).to_sec() self.sim.step(self.car_cmd, delta_t) @@ -401,31 +301,17 @@ class Simulator(object): False: An error is thrown if variable does not exist. @return: None or Exception. """ -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - rclpy_param = self.node.get_parameter_or(path, secondParam) - if not rclpy_param: - if returnValue: -======== #rospy_param = rospy.get_param(path, secondParam) rclpy_param = self.node.declare_parameter(path, secondParam).value if not rclpy_param: if returnValue: #rospy.logfatal(f'invalid file path: {path}') ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + self.node.get_logger().fatal(f'invalid file path: {path}') return None else: if '~' in path: if secondParam is not None: -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - - return self.node.declare_parameter_or(f'{path}', secondParam) - else: - - return self.node.declare_parameter(f'{path}') - - type_rclpy = type(rclpy_param) -======== #return rospy.get_param(f'{path}', secondParam) return self.node.declare_parameter(f'{path}', secondParam) else: @@ -434,7 +320,7 @@ class Simulator(object): return self.node.declare_parameter(f'{path}') type_rospy = type(rclpy_param) ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + if type_rclpy == dict: if secondParam is None: @@ -442,15 +328,6 @@ class Simulator(object): # min_param = (float)(rospy.get_param(f'{path}/min')) # max_param = (float)(rospy.get_param(f'{path}/max')) -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - min_param = (float)(self.node.get_parameter(f'{path}/min')) - max_param = (float)(self.node.get_parameter(f'{path}/max')) - else: - min_param = (float)(self.node.get_parameter_or( - f'{path}/min', secondParam)) - max_param = (float)(self.node.get_parameter_or( - f'{path}/max', secondParam)) -======== min_param = (float)(self.node.declare_parameter(f'{path}/min')) max_param = (float)(self.node.declare_parameter(f'{path}/max')) else: @@ -458,56 +335,28 @@ class Simulator(object): # f'{path}/min', secondParam)) # max_param = (float)(rospy.get_param( # f'{path}/max', secondParam)) ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + min_param = (float)(self.node.declare_parameter(f'{path}/min', secondParam)) max_param = (float)(self.node.declare_parameter(f'{path}/max', secondParam)) if not max_param: if returnValue: -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator -======== - #rospy.logfatal(f'invalid file path: {path}/max') ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator - self.node.get_logger().fatal(f'invalid file path: {path}/max') - return None - if not min_param: - if returnValue: -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator -======== #rospy.logfatal(f'invalid file path: {path}/min') ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + self.node.get_logger().fatal(f'invalid file path: {path}/min') return None # accounting for bugs in yaml file if min_param > max_param: param_val =(float)(random.uniform(max_param, min_param)) -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator -======== #rospy.set_param(f'{path}',param_val) ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator self.node.declare_parameter(f'{path}',param_val) - return param_val - else: - param_val =(float)(random.uniform(min_param, max_param)) -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - self.node.declare_parameter(f'{path}',param_val) -======== - #rospy.set_param(f'{path}',param_val) - self.node.declare_parameter(f'{path}',param_val) - ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + return param_val elif type_rclpy == float or type_rclpy == int: if secondParam is not None: -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - return self.node.declare_parameter(path, secondParam) - else: - return self.node.declare_parameter(path) - if returnValue: -======== #return rospy.get_param(path, secondParam) return self.node.declare_parameter(path, secondParam) else: @@ -515,7 +364,7 @@ class Simulator(object): return self.node.declare_parameter(path) if returnValue: #rospy.logfatal(f'invalid file path: {path}') ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + self.node.get_logger().fatal(f'invalid file path: {path}') return None @@ -555,12 +404,9 @@ class Simulator(object): init_pose = self.get_sim_param('~cars/init_pose') if 'name' not in car_config: -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - rclpy.shutdown('no "name" set for car config in sim') -======== #rospy.signal_shutdown('no "name" set for car config in sim') self.destroy_node('no "name" set for car config in sim') ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + car_name = car_config['name'] if 'randomize_pose' in car_config: init_pose = None @@ -574,13 +420,6 @@ class Simulator(object): 'car', init_pose=init_pose, noise=self.car_noise, car_props=self.car_properties) car_id = self.car_ids[car_name] # create the car's Subscribers -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - self.car_effort_subs[car_name] = rclpy.Subscriber( - f'/cars/{car_name}/effort', ControlEffort, - self.effort_cb, callback_args=car_id) - self.car_cmd_subs[car_name] = rclpy.Subscriber( - f'/cars/{car_name}/command', ControlCommand, -======== # self.car_effort_subs[car_name] = rospy.Subscriber( # f'/cars/{car_name}/effort', ControlEffort, # self.effort_cb, callback_args=car_id) @@ -592,22 +431,11 @@ class Simulator(object): # self.cmd_cb, callback_args=car_id) self.car_cmd_subs[car_name] = node.create_subscription( ControlCommand, f'/cars/{car_name}/command', ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + self.cmd_cb, callback_args=car_id) # create the car's Publishers if self.mode == SimulatorMode.REALISTIC: -<<<<<<<< HEAD:rktl_sim_backup/nodes/simulator - self.car_pose_pubs[car_name] = rclpy.Publisher( - f'/cars/{car_name}/pose_sync_early', - PoseWithCovarianceStamped, queue_size=1) - self.car_odom_pubs[car_name] = rclpy.Publisher( - f'/cars/{car_name}/odom_truth', Odometry, queue_size=1) - elif self.mode == SimulatorMode.IDEAL: - self.car_odom_pubs[car_name] = rclpy.Publisher( - f'/cars/{car_name}/odom', Odometry, queue_size=1) - -======== # self.car_pose_pubs[car_name] = rospy.Publisher( # f'/cars/{car_name}/pose_sync_early', # PoseWithCovarianceStamped, queue_size=1) @@ -625,7 +453,7 @@ class Simulator(object): # f'/cars/{car_name}/odom', Odometry, queue_size=1) self.car_odom_pubs[car_name] = node.create_publisher( Odometry, f'/cars/{car_name}/odom', 1) ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/simulator + return True diff --git a/src/rktl_sim/rktl_sim/nodes/visualizer b/src/rktl_sim/rktl_sim/nodes/visualizer index d9d0c8d8..594163a1 100755 --- a/src/rktl_sim/rktl_sim/nodes/visualizer +++ b/src/rktl_sim/rktl_sim/nodes/visualizer @@ -14,13 +14,10 @@ import math from std_msgs.msg import Float32 from nav_msgs.msg import Odometry import rclpy -<<<<<<<< HEAD:rktl_sim_backup/nodes/visualizer -from tf.transformations import euler_from_quaternion -======== import sys from rktl_dependencies.transformations import euler_from_quaternion ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/visualizer + from threading import Lock import numpy as np @@ -35,16 +32,6 @@ class VisualizerROS(object): """ROS wrapper for the visualizer.""" def __init__(self): -<<<<<<<< HEAD:rktl_sim_backup/nodes/visualizer - super().__init__("visualizer") - - # Collecting global parameters - field_width = self.get_parameter('/field/width') - field_length = self.get_parameter('/field/length') - goal_width = self.get_parameter('/field/goal/width') - wall_thickness = self.get_parameter('/field/wall_thickness') - ball_radius = self.get_parameter("/ball/radius") -======== #rospy.init_node("visualizer") rclpy.init(args=sys.argv) global node @@ -62,26 +49,12 @@ class VisualizerROS(object): wall_thickness = node.declare_parameter('/field/wall_thickness').value ball_radius = node.declare_parameter("/ball/radius").value ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/visualizer + # Creating pygame render self.window = visualizer.Window( field_width, field_length, wall_thickness, -<<<<<<<< HEAD:rktl_sim_backup/nodes/visualizer - node.get_parameter_or('~window_name', 'Rocket League Visualizer')) - - # Collecting private parameters - self.frame_id = node.get_parameter_or("~frame_id", "map") - self.timeout = node.get_parameter_or("~timeout", 10) - rate = node.create_rate(node.get_parameter_or("~rate", 20)) - car_width = node.get_parameter_or("~cars/body_width") - car_length = node.get_parameter_or("~cars/body_length") - - # Setting up field - id = 0 - field_img_path = node.get_parameter_or("~media/field", None) -======== # rospy.get_param('~window_name', 'Rocket League Visualizer')) node.declare_parameter('~window_name', 'Rocket League Visualizer').value) @@ -101,7 +74,7 @@ class VisualizerROS(object): id = 0 # field_img_path = rospy.get_param("~media/field", None) field_img_path = node.declare_parameter("~media/field", None).value ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/visualizer + if field_img_path is not None: self.window.createAsset( id, field_width, field_length, imgPath=field_img_path, @@ -145,13 +118,10 @@ class VisualizerROS(object): id += 1 # Setting up car -<<<<<<<< HEAD:rktl_sim_backup/nodes/visualizer - car_img_path = node.get_parameter_or("~media/car", None) -======== # car_img_path = rospy.get_param("~media/car", None) car_img_path = node.declare_parameter("~media/car", None).value ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/visualizer + if car_img_path is not None: self.car_id = id self.window.createAsset( @@ -159,12 +129,9 @@ class VisualizerROS(object): id += 1 # Setting up ball -<<<<<<<< HEAD:rktl_sim_backup/nodes/visualizer - ball_img_path = node.get_parameter_or("~media/ball", None) -======== # ball_img_path = rospy.get_param("~media/ball", None) ball_img_path = node.declare_parameter("~media/ball", None).value ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/visualizer + if ball_img_path is not None: self.ball_id = id self.window.createAsset( @@ -197,15 +164,6 @@ class VisualizerROS(object): self.path = None # Subscribers -<<<<<<<< HEAD:rktl_sim_backup/nodes/visualizer - node.create_subscription(Odometry, "/ball/odom", self.ball_odom_cb) - node.create_subscription(Odometry, "/cars/car0/odom", self.car_odom_cb) - node.create_subscription(Path, "/cars/car0/path", self.path_arr_cb) - node.create_subscription(Float32, "/cars/car0/lookahead_pnt", self.lookahead_cb - ) - node.create_subscription(BezierPathList, "/agents/agent0/bezier_path", self.bezier_path_cb) - -======== # rospy.Subscriber("/ball/odom", Odometry, self.ball_odom_cb) # rospy.Subscriber("/cars/car0/odom", Odometry, self.car_odom_cb) # rospy.Subscriber("/cars/car0/path", Path, self.path_arr_cb) @@ -218,7 +176,7 @@ class VisualizerROS(object): node.create_subscription(Float32, "/cars/car0/lookahead_pnt", self.lookahead_cb) node.create_subscription(BezierPathList, "/agents/agent0/bezier_path", self.bezier_path_cb) ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/visualizer + while not rclpy.ok(): try: self.window.show() @@ -226,11 +184,9 @@ class VisualizerROS(object): exit() try: rate.sleep() -<<<<<<<< HEAD:rktl_sim_backup/nodes/visualizer -======== except rclpy.ROSInterruptException: pass ->>>>>>>> ros-2-sim-magic:src/rktl_sim/rktl_sim/nodes/visualizer + def car_odom_cb(self, odom_msg): x = odom_msg.pose.pose.position.x