diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 0c23e4ee..43fb3e1b 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -3,7 +3,7 @@ - + diff --git a/code/acting/src/acting/acting.py b/code/acting/src/acting/acting.py deleted file mode 100755 index 19bffa54..00000000 --- a/code/acting/src/acting/acting.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python -import ros_compatibility as roscomp -from ros_compatibility.node import CompatibleNode -from carla_msgs.msg import CarlaEgoVehicleControl -from rospy import Publisher -from ros_compatibility.qos import QoSProfile, DurabilityPolicy -from std_msgs.msg import Bool - - -class Acting(CompatibleNode): - def __init__(self): - super(Acting, self).__init__('acting') - self.loginfo('Acting node started') - - self.control_loop_rate = self.get_param('control_loop_rate', 0.05) - self.role_name = self.get_param('role_name', 'ego_vehicle') - - self.control_publisher: Publisher = self.new_publisher( - CarlaEgoVehicleControl, - f'/carla/{self.role_name}/vehicle_control_cmd', - qos_profile=10 - ) - self.status_pub = self.new_publisher( - Bool, f"/carla/{self.role_name}/status", - qos_profile=QoSProfile( - depth=1, - durability=DurabilityPolicy.TRANSIENT_LOCAL) - ) - - def run(self): - """ - Starts the main loop of the node - :return: - """ - self.status_pub.publish(True) - self.loginfo('Acting node running') - - def loop(timer_event=None): - """ - Main loop of the acting node - :param timer_event: Timer event from ROS - :return: - """ - message = CarlaEgoVehicleControl() - - # set throttle to 0.1 - message.throttle = 0.1 - message.brake = 0 - message.hand_brake = False - message.manual_gear_shift = False - - message.header.stamp = roscomp.ros_timestamp(self.get_time(), - from_sec=True) - self.control_publisher.publish(message) - - self.new_timer(self.control_loop_rate, loop) - self.spin() - - -def main(args=None): - """ - main function starts the acting node - :param args: - """ - roscomp.init('acting', args=args) - - try: - node = Acting() - node.run() - except KeyboardInterrupt: - pass - finally: - roscomp.shutdown() - - -if __name__ == '__main__': - main() diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py new file mode 100755 index 00000000..938d4b1e --- /dev/null +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python +import ros_compatibility as roscomp +# from carla_msgs.msg import CarlaGnssRoute +from ros_compatibility.node import CompatibleNode +from ros_compatibility.qos import QoSProfile, DurabilityPolicy +from rospy import Publisher, Subscriber +from std_msgs.msg import Float32, Bool + + +# todo: docs +class PurePursuitController(CompatibleNode): + def __init__(self): + super(PurePursuitController, self).__init__('pure_pursuit_controller') + self.loginfo('PurePursuitController node started') + + self.control_loop_rate = self.get_param('control_loop_rate', 0.05) + self.role_name = self.get_param('role_name', 'ego_vehicle') + + self.stanley_steer_sub: Subscriber = self.new_subscription( + Float32, + f"/carla/{self.role_name}/max_velocity", + self.__get_max_velocity, + qos_profile=1) + + # self.position_sub: Subscriber = self.new_subscription( + # GNSSMeasurement, + # f"/carla/{self.role_name}/Speed", + # self.__get_velocity, + # qos_profile=1) + + self.pure_pursuit_steer_pub: Publisher = self.new_publisher( + Float32, + f"/carla/{self.role_name}/pure_pursuit_steer", + qos_profile=1) + + self.status_pub = self.new_publisher( # todo: delete (this is only + # necessary if vehicle controller isn't running) + Bool, f"/carla/{self.role_name}/status", + qos_profile=QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL) + ) + + self.__current_velocity: float = 0 + self.__max_velocity: float = 0 + + def run(self): + """ + Starts the main loop of the node + :return: + """ + self.loginfo('PurePursuitController node running') + self.status_pub.publish(True) # todo: delete (this is only + # necessary if vehicle controller isn't running) + + def loop(timer_event=None): + """ + Main loop of the acting node + :param timer_event: Timer event from ROS + :return: + """ + pass + + self.new_timer(self.control_loop_rate, loop) + self.spin() + + +def main(args=None): + """ + main function starts the acting node + :param args: + """ + roscomp.init('pure_pursuit_controller', args=args) + + try: + node = PurePursuitController() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() + + +if __name__ == '__main__': + main()