Skip to content

Commit

Permalink
fixed merge conflict
Browse files Browse the repository at this point in the history
  • Loading branch information
rtjord committed Apr 7, 2024
2 parents 7e1e035 + 39c65b1 commit 3986b17
Show file tree
Hide file tree
Showing 16 changed files with 65 additions and 55 deletions.
4 changes: 2 additions & 2 deletions src/rktl_autonomy/rktl_autonomy/_ros_interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -87,13 +87,13 @@ def __init__(self, node_name='gym_interface', eval=False, launch_file=None, laun
# initialize self
os.environ['ROS_MASTER_URI'] = f'http://localhost:{port}'
# rospy.init_node(node_name)
self.node = rclpy.Node(node_name)
self.node = rclpy.create_node(node_name)
# let someone else take a turn
os.remove(f'/tmp/{run_id}_launch')
else:
# use an existing ROS network
# rospy.init_node(node_name)
self.node = rclpy.Node(node_name)
self.node = rclpy.create_node(node_name)

# private variables
self._cond = Condition()
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_autonomy/rktl_autonomy/plotter.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
import matplotlib.pyplot as plt
from numpy import append
from os.path import expanduser, normpath
class Plotter(rclpy.Node):
class Plotter(rclpy.create_node):
"""Plot progress during training."""
def __init__(self):
#rospy.init_node('model_progress_plotter')
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_autonomy/rktl_autonomy/rocket_league_interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ def __init__(self, eval=False, launch_file=('rktl_autonomy', 'rocket_league_trai

# Publishers
self._command_pub = self.node.create_publisher(ControlCommand, 'cars/car0/command', 1)
self._reset_srv = self.node.create_client(Empty, 'sim_reset')
self._reset_srv = self.node.create_client(Empty, 'sim_reset', qos_profile=1)

# State variables
self._car_odom = None
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_autonomy/test/test_step_node
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ class TestStep(unittest.TestCase):
def test_all(self):
# initialize node and interface to code under test
#rospy.init_node('rocket_league_agent')
self.node = rclpy.Node('rocket_league_agent')
self.node = rclpy.create_node('rocket_league_agent')

#self.car_pub = rospy.Publisher('cars/car0/odom', Odometry, queue_size=1)
self.car_pub = self.node.create_publisher(Odometry, 'cars/car0/odom', 1)
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_control/rktl_control/keyboard_interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ def main():
rclpy.init(args=sys.argv)
node = rclpy.create_node('keyboard')

effort_pub = node.create_publisher(ControlEffort, 'effort', queue_size=1)
effort_pub = node.create_publisher(ControlEffort, 'effort')
reset_srv = node.create_client(Empty, '/sim_reset')

wrapper(main)
Expand Down
4 changes: 2 additions & 2 deletions src/rktl_control/rktl_control/topic_delay.py
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,8 @@ def __init__(self):
# msg_type = type
# assert msg_type is not None

pub = self.node.create_publisher(topic_type, output_name, qos_profile=1)
self.node.create_subscription(topic_type, input_name, self.msg_cb, qos_profile=1)
pub = self.node.create_publisher(topic_type, output_name, 1)
self.node.create_subscription(topic_type, input_name, self.msg_cb, 1)
self.buffer = deque()

while rclpy.ok():
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_control/test/test_mean_filter_node
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ class TestMeanFilter(unittest.TestCase):
rclpy.init(args=sys.argv)
self.node = rclpy.create_node('test_mean_filter_node')

pub = self.node.create_publisher(PoseWithCovarianceStamped, 'pose_sync', queue_size=1)
pub = self.node.create_publisher(PoseWithCovarianceStamped, 'pose_sync')
self.node.create_subscription(Odometry, 'odom', self.odom_cb)

# member variables used for test
Expand Down
6 changes: 3 additions & 3 deletions src/rktl_control/test/test_sync_node
Original file line number Diff line number Diff line change
Expand Up @@ -47,9 +47,9 @@ class TestSync(unittest.TestCase):
rclpy.init(args=sys.args)
self.node = rclpy.create_node('test_sync_node')

car0_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'cars/car0/pose', queue_size=0, latch=True)
car1_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'cars/car1/pose', queue_size=0, latch=True)
ball_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'ball/pose', queue_size=0, latch=True)
car0_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'cars/car0/pose')
car1_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'cars/car1/pose')
ball_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'ball/pose')

self.node.create_subscription(PoseWithCovarianceStamped, 'cars/car0/pose_sync', self.car0_cb)
self.node.create_subscription(PoseWithCovarianceStamped, 'cars/car1/pose_sync', self.car1_cb)
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_perception/nodes/pose_to_tf
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ from geometry_msgs.msg import PoseWithCovarianceStamped, TransformStamped
from tf2_ros import TransformBroadcaster


class Publisher(rclpy.Node):
class Publisher(rclpy.create_node):
"""Workaround class until TF is implemented"""

def __init__(self):
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_perception/nodes/projector
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ from rktl_dependencies.transformations import translation_matrix, quaternion_mat
import numpy as np


class Projector(rclpy.Node):
class Projector(rclpy.create_node):
"""Class to synchronize and buffer all poses."""

def __init__(self):
Expand Down
6 changes: 3 additions & 3 deletions src/rktl_planner/rktl_planner/nodes/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,9 @@
All rights reserved.
"""

from path_follower import PathFollower
from path_planner import PathPlanner
from patrol_planner import PatrolPlanner
from .path_follower import PathFollower
from .path_planner import PathPlanner
from .patrol_planner import PatrolPlanner

__all__ = [
"PathFollower",
Expand Down
13 changes: 8 additions & 5 deletions src/rktl_planner/rktl_planner/nodes/bezier_path_server.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
import rclpy
import math
from rktl_dependencies.transformations import euler_from_quaternion, quaternion_from_euler
from rktl_planner.srv import CreateBezierPath, CreateBezierPathRequest, CreateBezierPathResponse
from rktl_msgs.srv import CreateBezierPath
from rktl_planner import BezierPath
from rktl_msgs.msg import Path as PathMsg, Waypoint as WaypointMsg
from geometry_msgs.msg import Pose, Point, Vector3
Expand Down Expand Up @@ -53,7 +53,7 @@ def create_segment(start_pose: Pose, end_pose: Pose, velocity: float,
return segments


def bezier_path_server(req: CreateBezierPathRequest):
def bezier_path_server(req: CreateBezierPath.Request):
velocity = req.velocity
bezier_segments = []
durations = []
Expand All @@ -70,7 +70,7 @@ def bezier_path_server(req: CreateBezierPathRequest):
else:
durations.append(segment.duration.to_sec())

res = CreateBezierPathResponse()
res = CreateBezierPath.Response()
res.bezier_paths = [x.to_msg() for x in bezier_segments]
res.linear_path = PathMsg()
res.linear_path.velocity = velocity
Expand All @@ -96,8 +96,11 @@ def bezier_path_server(req: CreateBezierPathRequest):
idx += 1
return res

if __name__ == '__main__':
def main():
rclpy.init()
node = rclpy.create_node('bezier_path_server')
service = node.create_service(CreateBezierPath, 'create_bezier_path', bezier_path_server)
rclpy.spin()
rclpy.spin()

if __name__ == '__main__':
main()
21 changes: 11 additions & 10 deletions src/rktl_planner/rktl_planner/nodes/path_follower.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,32 +26,31 @@ class PathFollower(object):
def __init__(self):
rclpy.init()
global node
node = rclpy.Node('path_follower')
node = rclpy.create_node('path_follower')

self.path_start_time = None
self.path = None
self.last_pose_idx = None
self.start_time = None
self.max_speed = None

self.frame_id = node.declare_parameter('~frame_id', 'map')
self.frame_id = node.get_parameter_or('~frame_id', rclpy.Parameter('map')).get_parameter_value().string_value

# Max speed to travel path
self.max_speed = node.declare_parameter('~max_speed', 0.1)
self.max_speed = node.get_parameter_or('~max_speed', rclpy.Parameter(0.1)).get_parameter_value().double_value

# Radius to search for intersections
self.lookahead_dist = node.declare_parameter('~lookahead_dist', 0.15)
self.lookahead_dist = node.get_parameter_or('~lookahead_dist', rclpy.Parameter(0.15)).get_parameter_value().double_value

# Coeffient to adjust lookahead distance by speed
self.lookahead_gain = node.declare_parameter('~lookahead_gain', 0.035)
self.lookahead_gain = node.get_parameter_or('~lookahead_gain', rclpy.Parameter(.055)).get_parameter_value().double_value

# Number of waypoints to search per pass (-1 is full path)
self.lookahead_pnts = node.declare_parameter('~lookahead_pnts', -1)
self.lookahead_pnts = node.get_parameter_or('~lookahead_pnts', rclpy.Parameter(-1)).get_parameter_value().integer_value

# Publishers
car_name = node.declare_parameter('~car_name')
self.bot_velocity_cmd = node.create_publisher(ControlCommand, f'/cars/{car_name}/command', 1)

car_name = node.get_parameter('~car_name').get_parameter_value().string_value

# Subscribers
node.create_subscription(Odometry, f'/cars/{car_name}/odom', self.odom_cb, qos_profile=1)
node.create_subscription(Path, 'linear_path', self.path_cb, qos_profile=1)
Expand Down Expand Up @@ -137,6 +136,8 @@ def odom_cb(self, odom_msg: Odometry):
cmd.curvature = -(1.0 / turn_rad)
self.bot_velocity_cmd.publish(cmd)

def main():
PathFollower()

if __name__ == "__main__":
PathFollower()
main()
5 changes: 4 additions & 1 deletion src/rktl_planner/rktl_planner/nodes/path_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -148,5 +148,8 @@ def reset(self, _: Empty_Request):
self.bezier_path_pub.publish(bezier_path_list)
return Empty_Response()

if __name__ == '__main__':
def main():
PathPlanner()

if __name__ == '__main__':
main()
7 changes: 5 additions & 2 deletions src/rktl_planner/rktl_planner/nodes/patrol_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
from rktl_msgs.msg import ControlCommand

import math, time
from rktl_dependencies.rktl_dependencies.angles import shortest_angular_distance as sad
from rktl_dependencies.angles import shortest_angular_distance as sad

class PatrolPlanner(object):
"""A very simple strategy for rktl. Patrol around the field in a circle and
Expand Down Expand Up @@ -285,5 +285,8 @@ def control_curvature(self, error):
self.prev_error = error
return curvature

if __name__ == "__main__":
def main():
PatrolPlanner()

if __name__ == "__main__":
main()
40 changes: 20 additions & 20 deletions src/rktl_sim/rktl_sim/sim_simulator.py
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,8 @@ def __init__(self):
# setting config parameters (stay constant for the whole simulator run)
# rospy.init_node('simulator')
rclpy.init(args=sys.argv)
global node
node = rclpy.create_node("simulator")
#global node
self.node = rclpy.create_node("simulator")


mode = self.get_sim_param('~mode')
Expand All @@ -73,31 +73,31 @@ def __init__(self):
self.mode = SimulatorMode.REALISTIC
else:
#rospy.signal_shutdown('unknown sim mode set "{}"'.format(mode))
self.destroy_node('unknown sim mode set "{}"'.format(mode))
self.node.destroy_node()#f'unknown sim mode set "{mode}"')


self.cmd_lock = Lock()
self.reset_lock = Lock()
self.car_cmd = (0.0, 0.0)

render_enabled = self.get_sim_param('~render', secondParam=False)
#render_enabled = self.get_sim_param('~render', secondParam=False)
#self.status_pub = rospy.Publisher('match_status', MatchStatus, queue_size=1)
self.status_pub = node.create_publisher(MatchStatus, 'match_status', 1)
self.status_pub = self.node.create_publisher(MatchStatus, 'match_status', 1)
self.ball_pose_pub, self.ball_odom_pub = None, None
if self.mode == SimulatorMode.REALISTIC:
#self.ball_pose_pub = rospy.Publisher('/ball/pose_sync_early', PoseWithCovarianceStamped, queue_size=1)
self.ball_pose_pub = node.create_publisher(PoseWithCovarianceStamped, '/ball/pose_sync_early', 1)
self.ball_pose_pub = self.node.create_publisher(PoseWithCovarianceStamped, '/ball/pose_sync_early', 1)
#self.ball_odom_pub = rospy.Publisher('/ball/odom_truth', Odometry, queue_size=1)
self.ball_odom_pub = node.create_publisher(Odometry, '/ball/odom_truth', 1)
self.ball_odom_pub = self.node.create_publisher(Odometry, '/ball/odom_truth', 1)
elif self.mode == SimulatorMode.IDEAL:
#self.ball_odom_pub = rospy.Publisher('/ball/odom', Odometry, queue_size=1)
self.ball_odom_pub = node.create_publisher(Odometry, '/ball/odom', 1)
self.ball_odom_pub = self.node.create_publisher(Odometry, '/ball/odom', 1)

# Services
# rospy.Service('sim_reset_car_and_ball', Empty, self.reset_cb)
# 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)
self.node.create_service(Empty, 'sim_reset_car_and_ball', self.reset_cb)
self.node.create_service(Empty, 'sim_reset_ball', self.reset_ball_cb)


while not rclpy.ok():
Expand All @@ -113,11 +113,11 @@ def check_urdf(self, urdf_path):

if urdf_path is None:
#rospy.signal_shutdown('no urdf path set for "{}"'.format(urdf_path))
self.destroy_node('no urdf path set for "{}"'.format(urdf_path))
self.node.destroy_node()
try:
raise Exception('no urdf path set for "{}"'.format(urdf_path))
except Exception as e:
node.get_logger().error(str(e))
self.node.get_logger().error(str(e))
rclpy.shutdown()
i = 0
while not os.path.isfile(urdf_path) and i < 5:
Expand All @@ -127,7 +127,7 @@ def check_urdf(self, urdf_path):

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))
self.node.destroy_node()


def effort_cb(self, effort_msg, car_id):
Expand Down Expand Up @@ -318,7 +318,7 @@ def get_sim_param(self, path, returnValue=False, secondParam=None):
#return rospy.get_param(f'{path}')
return self.node.declare_parameter(f'{path}')

type_rospy = type(rclpy_param)
type_rclpy = type(rclpy_param)


if type_rclpy == dict:
Expand Down Expand Up @@ -404,7 +404,7 @@ def update_all_cars(self):

if 'name' not in car_config:
#rospy.signal_shutdown('no "name" set for car config in sim')
self.destroy_node('no "name" set for car config in sim')
self.node.destroy_node()

car_name = car_config['name']
if 'randomize_pose' in car_config:
Expand All @@ -422,13 +422,13 @@ def update_all_cars(self):
# self.car_effort_subs[car_name] = rospy.Subscriber(
# f'/cars/{car_name}/effort', ControlEffort,
# self.effort_cb, callback_args=car_id)
self.car_effort_subs[car_name] = node.create_subscription(
self.car_effort_subs[car_name] = self.node.create_subscription(
ControlEffort, f'/cars/{car_name}/effort',
self.effort_cb, callback_args=car_id)
# self.car_cmd_subs[car_name] = rospy.Subscriber(
# f'/cars/{car_name}/command', ControlCommand,
# self.cmd_cb, callback_args=car_id)
self.car_cmd_subs[car_name] = node.create_subscription(
self.car_cmd_subs[car_name] = self.node.create_subscription(
ControlCommand, f'/cars/{car_name}/command',

self.cmd_cb, callback_args=car_id)
Expand All @@ -438,19 +438,19 @@ def update_all_cars(self):
# self.car_pose_pubs[car_name] = rospy.Publisher(
# f'/cars/{car_name}/pose_sync_early',
# PoseWithCovarianceStamped, queue_size=1)
self.car_pose_pubs[car_name] = node.create_publisher(
self.car_pose_pubs[car_name] = self.node.create_publisher(
PoseWithCovarianceStamped, f'/cars/{car_name}/pose_sync_early',
1)

# self.car_odom_pubs[car_name] = rospy.Publisher(
# f'/cars/{car_name}/odom_truth', Odometry, queue_size=1)
self.car_pose_pubs[car_name] = node.create_publisher(
self.car_pose_pubs[car_name] = self.node.create_publisher(
Odometry, f'/cars/{car_name}/odom_truth', 1)

elif self.mode == SimulatorMode.IDEAL:
# self.car_odom_pubs[car_name] = rospy.Publisher(
# f'/cars/{car_name}/odom', Odometry, queue_size=1)
self.car_odom_pubs[car_name] = node.create_publisher(
self.car_odom_pubs[car_name] = self.node.create_publisher(
Odometry, f'/cars/{car_name}/odom', 1)

return True
Expand Down

0 comments on commit 3986b17

Please sign in to comment.