Skip to content

Commit

Permalink
Merge branch 'ros-2-complete' of https://github.com/purdue-arc/rocket…
Browse files Browse the repository at this point in the history 
…_league into ros-2-complete
  • Loading branch information
@kowals14
kowals14 committed Feb 11, 2024
2 parents 955133e + 3987359 commit 71670d2
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 241 deletions.
2 changes: 1 addition & 1 deletion src/rktl_control/rktl_control/particle_odom_filter.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_perception/nodes/projector
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
204 changes: 16 additions & 188 deletions src/rktl_sim/rktl_sim/nodes/simulator
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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:
Expand All @@ -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)."""
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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:
Expand All @@ -434,88 +320,51 @@ 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:

# 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:
# min_param = (float)(rospy.get_param(
# 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:
#return rospy.get_param(path)
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

Expand Down Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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


Expand Down
Loading

0 comments on commit 71670d2

Please sign in to comment.