-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'main' into 490-feature-remodel-acc-concept
- Loading branch information
Showing
52 changed files
with
1,086 additions
and
283 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,41 +1,21 @@ | ||
| <!----> | ||
| <launch> | ||
| <arg name="role_name" default="hero" /> | ||
| <arg name="control_loop_rate" default="0.05" /> | ||
| <arg name="control_loop_rate" default="0.05" /> | ||
|
|
||
| <node pkg="acting" type="velocity_controller.py" name="velocity_controller" output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
|
|
||
| <node pkg="acting" type="stanley_controller.py" name="stanley_controller" output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| <!--Passthrough | ||
| all messages the control package needs--> | ||
| <node pkg="acting" type="passthrough.py" name="passthrough" output="screen"> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
|
|
||
| <node pkg="acting" type="pure_pursuit_controller.py" name="pure_pursuit_controller" output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
|
|
||
| <node pkg="acting" type="vehicle_controller.py" name="vehicle_controller" output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> <!-- leaderboard expects commands every 0.05 seconds OTHERWISE IT LAGS REALLY BADLY--> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
|
|
||
| <node pkg="acting" type="MainFramePublisher.py" name="MainFramePublisher" output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
|
|
||
| <!-- UNCOMMENT THIS TO USE THE DEBUG_NODE FOR ACTING-TESTING --> | ||
| <!-- <node pkg="acting" type="Acting_Debug_Node.py" name="Acting_Debug_Node" output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
| --> | ||
|
|
||
| <!-- If you want a live rqt_plots to show up automatically, include them like following example-plot for Velocity-Controller-Testing --> | ||
| <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="velocity_test" args="/carla/hero/Speed /paf/hero/target_velocity /paf/hero/throttle /paf/hero/brake"/--> | ||
|
|
||
| </launch> | ||
| <include file="$(find control)/launch/control.launch"> | ||
| <arg name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| </include> | ||
| </launch> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,87 @@ | ||
| #!/usr/bin/env python | ||
|
|
||
| import ros_compatibility as roscomp | ||
| from ros_compatibility.node import CompatibleNode | ||
| from rospy import Publisher, Subscriber | ||
| from std_msgs.msg import Float32 | ||
| from geometry_msgs.msg import PoseStamped | ||
| from nav_msgs.msg import Path | ||
|
|
||
|
|
||
| from dataclasses import dataclass | ||
| from typing import Type, Dict | ||
|
|
||
|
|
||
| @dataclass | ||
| class TopicMapping: | ||
| pub_name: str | ||
| sub_name: str | ||
| topic_type: Type | ||
|
|
||
|
|
||
| class Passthrough(CompatibleNode): | ||
| """This nodes sole purpose is to pass through all messages that control needs. | ||
| The purpose of this is that Control-Package should not have any global dependencies, | ||
| but is only dependent on the acting package. | ||
| """ | ||
|
|
||
| role_name = "hero" # Legacy will change soon | ||
|
|
||
| # Topics for velocity controller. | ||
| target_velocity = TopicMapping( | ||
| pub_name="/paf/acting/target_velocity", | ||
| sub_name=f"/paf/{role_name}/target_velocity", | ||
| topic_type=Float32, | ||
| ) | ||
| # Topics for steering controllers | ||
| trajectory = TopicMapping( | ||
| pub_name="/paf/acting/trajectory", | ||
| sub_name=f"/paf/{role_name}/trajectory", | ||
| topic_type=Path, | ||
| ) | ||
| position = TopicMapping( | ||
| pub_name="/paf/acting/current_pos", | ||
| sub_name=f"/paf/{role_name}/current_pos", | ||
| topic_type=PoseStamped, | ||
| ) | ||
| heading = TopicMapping( | ||
| pub_name="/paf/acting/current_heading", | ||
| sub_name=f"/paf/{role_name}/current_heading", | ||
| topic_type=Float32, | ||
| ) | ||
|
|
||
| mapped_topics = [target_velocity, trajectory, position, heading] | ||
|
|
||
| def __init__(self): | ||
| self.publishers: Dict[str, Publisher] = {} | ||
| self.subscribers: Dict[str, Subscriber] = {} | ||
| for topic in self.mapped_topics: | ||
| self.publishers[topic.pub_name] = self.new_publisher( | ||
| topic.topic_type, topic.pub_name, qos_profile=1 | ||
| ) | ||
|
|
||
| self.subscribers[topic.pub_name] = self.new_subscription( | ||
| topic.topic_type, | ||
| topic.sub_name, | ||
| callback=self.publishers[topic.pub_name].publish, | ||
| qos_profile=1, | ||
| ) | ||
|
|
||
|
|
||
| def main(args=None): | ||
| """Start the node. | ||
| This is the entry point, if called by a launch file. | ||
| """ | ||
| roscomp.init("passthrough", args=args) | ||
|
|
||
| try: | ||
| node = Passthrough() | ||
| node.spin() | ||
| except KeyboardInterrupt: | ||
| pass | ||
| finally: | ||
| roscomp.shutdown() | ||
|
|
||
|
|
||
| if __name__ == "__main__": | ||
| main() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,18 @@ | ||
| cmake_minimum_required(VERSION 3.0.2) | ||
| project(control) | ||
|
|
||
|
|
||
| find_package(catkin REQUIRED) | ||
|
|
||
| catkin_python_setup() | ||
|
|
||
| find_package(catkin REQUIRED COMPONENTS | ||
| rospy | ||
| std_msgs | ||
| ) | ||
| catkin_package() | ||
|
|
||
|
|
||
| include_directories( | ||
| ) | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,28 @@ | ||
| <launch> | ||
| <arg name="role_name" default="hero" /> | ||
| <arg name="control_loop_rate" default="0.05" /> | ||
|
|
||
|
|
||
| <node pkg="control" type="velocity_controller.py" name="velocity_controller" output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
|
|
||
| <node pkg="control" type="stanley_controller.py" name="stanley_controller" output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
|
|
||
| <node pkg="control" type="pure_pursuit_controller.py" name="pure_pursuit_controller" | ||
| output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
|
|
||
| <node pkg="control" type="vehicle_controller.py" name="vehicle_controller" output="screen"> | ||
| <param name="control_loop_rate" value="$(arg control_loop_rate)" /> <!-- leaderboard expects | ||
| commands every 0.05 seconds OTHERWISE IT LAGS REALLY BADLY--> | ||
| <param name="role_name" value="$(arg role_name)" /> | ||
| </node> | ||
|
|
||
| </launch> |
Oops, something went wrong.