diff --git a/NaviGator/gnc/navigator_msg_multiplexer/CMakeLists.txt b/NaviGator/gnc/navigator_msg_multiplexer/CMakeLists.txt deleted file mode 100644 index 12624bfee..000000000 --- a/NaviGator/gnc/navigator_msg_multiplexer/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(navigator_msg_multiplexer) - -find_package(catkin REQUIRED COMPONENTS - rospy -) - -#add dynamic reconfigure api -find_package(catkin REQUIRED dynamic_reconfigure) -generate_dynamic_reconfigure_options( - cfg/OgridConfig.cfg -) - -catkin_package() diff --git a/NaviGator/gnc/navigator_msg_multiplexer/nodes/ogrid_arbiter.py b/NaviGator/gnc/navigator_msg_multiplexer/nodes/ogrid_arbiter.py index eb425ec7a..c31fb6724 100755 --- a/NaviGator/gnc/navigator_msg_multiplexer/nodes/ogrid_arbiter.py +++ b/NaviGator/gnc/navigator_msg_multiplexer/nodes/ogrid_arbiter.py @@ -7,7 +7,7 @@ import genpy import mil_tools import numpy as np -import rospy +import rclpy import tf.transformations as trns from dynamic_reconfigure.client import Client from dynamic_reconfigure.server import Server @@ -17,6 +17,8 @@ from navigator_path_planner import params from std_srvs.srv import Trigger +logger = rclpy.logging.get_logger("ogrid_arbiter") + # Wow what a concept def fprint(*args, **kwargs): @@ -155,7 +157,7 @@ class OGrid: nav_ogrid: Optional[OccupancyGrid] np_map: Optional[np.ndarray] replace: bool - subscriber: rospy.Subscriber + subscriber: rclpy.Subscriber def __init__(self, topic: str, replace: bool = False, frame_id: str = "enu"): # Assert that the topic is valid @@ -164,11 +166,11 @@ def __init__(self, topic: str, replace: bool = False, frame_id: str = "enu"): self.nav_ogrid = None # Last received OccupancyGrid message self.np_map = None # Numpy version of last received OccupancyGrid message self.replace = replace - self.subscriber = rospy.Subscriber( - topic, + self.subscriber = rclpy.create_subscription( OccupancyGrid, + topic, self.subscriber_callback, - queue_size=1, + 1, ) @property @@ -182,7 +184,7 @@ def callback_delta(self) -> float: """ if self.last_message_stamp is None: return 0 - return (rospy.Time.now() - self.last_message_stamp).to_sec() + return (rclpy.Time.now() - self.last_message_stamp).to_sec() def subscriber_callback(self, ogrid: OccupancyGrid): """ @@ -228,14 +230,14 @@ def __init__( self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion) self.global_ogrid = self.create_grid((map_size, map_size)) - rospy.Subscriber("/odom", Odometry, self.set_odom) - self.publisher = rospy.Publisher("/ogrid_master", OccupancyGrid, queue_size=1) + node.create_subscription(Odometry, "/odom", self.set_odom) + self.publisher = node.create_publisher(OccupancyGrid, "/ogrid_master", 1) self.ogrid_server = Server(OgridConfig, self.dynamic_config_callback) self.dynam_client = Client("bounds_server", config_callback=self.bounds_cb) - rospy.Service("~center_ogrid", Trigger, self.center_ogrid) - rospy.Timer(rospy.Duration(1.0 / rate), self.publish) + node.create_service(Trigger, "~center_ogrid", self.center_ogrid) + rclpy.Timer(rclpy.Duration(1.0 / rate), self.publish) def set_odom(self, msg: Odometry) -> np.ndarray: """ @@ -290,7 +292,7 @@ def bounds_cb(self, config: dict) -> None: config: dict - The update changes from dynamic reconfigure to be handled. """ - rospy.loginfo("BOUNDS UPDATED") + logger.info("BOUNDS UPDATED") self.enu_bounds = [ [config["x1"], config["y1"], 1], @@ -375,7 +377,7 @@ def create_grid(self, map_size: Tuple[int, int]) -> OccupancyGrid: to generate in the ogrid. """ ogrid = OccupancyGrid() - ogrid.header.stamp = rospy.Time.now() + ogrid.header.stamp = rclpy.Time.now() ogrid.header.frame_id = self.frame_id ogrid.info.resolution = self.resolution @@ -392,10 +394,10 @@ def create_grid(self, map_size: Tuple[int, int]) -> OccupancyGrid: def publish(self, timer_event): """ Publishes the final ogrid to the ROS topic. Called by - a rospy.Timer set to a specific rate. + a rclpy.Timer set to a specific rate. Args: - timer_event: The TimerEvent sent by the rospy Timer + timer_event: The TimerEvent sent by the rclpy Timer upon each call by the timer. """ global_ogrid = deepcopy(self.global_ogrid) @@ -565,6 +567,7 @@ def plow_snow(self, np_grid: np.ndarray, ogrid: OccupancyGrid) -> np.ndarray: if __name__ == "__main__": - rospy.init_node("ogrid_server", anonymous=False) + rclpy.init() + node = rclpy.create_node("ogrid_server") og_server = OGridServer() - rospy.spin() + rclpy.spin() diff --git a/NaviGator/gnc/navigator_msg_multiplexer/package.xml b/NaviGator/gnc/navigator_msg_multiplexer/package.xml index b90dc0814..120f60b11 100644 --- a/NaviGator/gnc/navigator_msg_multiplexer/package.xml +++ b/NaviGator/gnc/navigator_msg_multiplexer/package.xml @@ -1,10 +1,12 @@ - + navigator_msg_multiplexer 0.0.0 The navigator messgage multiplexer package zachgoins TODO catkin - + + ament_python +