Merge pull request #58 from rosflight/input-mapper

Input mapper
rosflight · Jun 26, 2024 · 13cbd1c · 13cbd1c
2 parents a00e15d + 6a0d510
commit 13cbd1c
Show file tree

Hide file tree

Showing 13 changed files with 796 additions and 36 deletions.
diff --git a/rosplane/CMakeLists.txt b/rosplane/CMakeLists.txt
@@ -33,59 +33,75 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 find_package(rosflight_msgs REQUIRED)
 pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
 
-ament_export_dependencies(rclcpp rclpy)
-ament_export_include_directories(include)
-
-#install(DIRECTORY originalFiles DESTINATION share/${PROJECT_NAME}/)
-include_directories(  #use this if you need .h files for include statements.  The include will need to have the directories where each .h is respectively.
+include_directories(
   include
+  include/param_manager
   ${EIGEN3_INCLUDE_DIRS}
   ${GAZEBO_INCLUDE_DIRS}
   ${YAML_CPP_INCLUDEDIR}
 )
 
+### EXTRA FILES TO INSTALL ###
+
 install(DIRECTORY launch params DESTINATION share/${PROJECT_NAME}/)
 
-### START OF REAL EXECUTABLES ###
+### LIBRARIES ###
+
+# Param Manager
+add_library(param_manager
+  include/param_manager/param_manager.hpp
+  src/param_manager/param_manager.cpp
+)
+ament_target_dependencies(param_manager rclcpp)
+ament_export_targets(param_manager HAS_LIBRARY_TARGET)
+install(DIRECTORY include/param_manager DESTINATION include)
+install(TARGETS param_manager
+  EXPORT param_manager
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+### START OF EXECUTABLES ###
 
 # Controller
 add_executable(rosplane_controller
-            src/controller_base.cpp
-            src/controller_state_machine.cpp
-            src/controller_successive_loop.cpp
-            src/controller_total_energy.cpp
-            src/param_manager.cpp)
+  src/controller_base.cpp
+  src/controller_state_machine.cpp
+  src/controller_successive_loop.cpp
+  src/controller_total_energy.cpp)
 ament_target_dependencies(rosplane_controller rosplane_msgs rosflight_msgs rclcpp rclpy Eigen3)
+target_link_libraries(rosplane_controller param_manager)
 install(TARGETS
   rosplane_controller
   DESTINATION lib/${PROJECT_NAME})
 
-
 # Follower
 add_executable(rosplane_path_follower
-            src/path_follower_example.cpp
-            src/path_follower_base.cpp
-            src/param_manager.cpp)
+  src/path_follower_example.cpp
+  src/path_follower_base.cpp)
 ament_target_dependencies(rosplane_path_follower rosplane_msgs rclcpp rclpy Eigen3)
+target_link_libraries(rosplane_path_follower param_manager)
 install(TARGETS
   rosplane_path_follower
   DESTINATION lib/${PROJECT_NAME})
 
 # Manager
 add_executable(rosplane_path_manager
-            src/path_manager_base.cpp
-            src/path_manager_example.cpp
-            src/param_manager.cpp)
+  src/path_manager_base.cpp
+  src/path_manager_example.cpp)
 ament_target_dependencies(rosplane_path_manager rosplane_msgs rclcpp rclpy Eigen3)
+target_link_libraries(rosplane_path_manager param_manager)
 install(TARGETS
   rosplane_path_manager
   DESTINATION lib/${PROJECT_NAME})
 
 # Planner
 add_executable(rosplane_path_planner
-            src/path_planner.cpp
-            src/param_manager.cpp)
+  src/path_planner.cpp)
 target_link_libraries(rosplane_path_planner
+  param_manager
   ${YAML_CPP_LIBRARIES}
 )
 ament_target_dependencies(rosplane_path_planner rosplane_msgs rosflight_msgs std_srvs rclcpp rclpy Eigen3)
@@ -95,16 +111,16 @@ install(TARGETS
 
 # Estimator
 add_executable(rosplane_estimator_node
-              src/estimator_base.cpp
-              src/estimator_example.cpp
-              src/param_manager.cpp)
+  src/estimator_base.cpp
+  src/estimator_example.cpp)
 target_link_libraries(rosplane_estimator_node
   ${YAML_CPP_LIBRARIES}
 )
 ament_target_dependencies(rosplane_estimator_node rosplane_msgs rosflight_msgs rclcpp Eigen3)
+target_link_libraries(rosplane_estimator_node param_manager)
 install(TARGETS
-        rosplane_estimator_node
-        DESTINATION lib/${PROJECT_NAME})
+  rosplane_estimator_node
+  DESTINATION lib/${PROJECT_NAME})
 
 #### END OF EXECUTABLES ###
 

diff --git a/rosplane/include/param_manager.hpp → ...e/include/param_manager/param_manager.hpp b/rosplane/include/param_manager.hpp → ...e/include/param_manager/param_manager.hpp
diff --git a/rosplane/launch/rosplane.launch.py b/rosplane/launch/rosplane.launch.py
@@ -1,9 +1,11 @@
 import os
 import sys
+import launch.actions
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 
+
 def generate_launch_description():
     # Create the package directory
     rosplane_dir = get_package_share_directory('rosplane')
@@ -16,39 +18,53 @@ def generate_launch_description():
     for arg in sys.argv:
         if arg.startswith("control_type:="):
             control_type = arg.split(":=")[1]
-        
+
         if arg.startswith("aircraft:="):
             aircraft = arg.split(":=")[1]
-        
+
         if arg.startswith("seed_estimator:="):
             use_params = arg.split(":=")[1].lower()
-    
+
     autopilot_params = os.path.join(
         rosplane_dir,
         'params',
         aircraft + '_autopilot_params.yaml'
     )
 
     return LaunchDescription([
+        launch.actions.DeclareLaunchArgument(
+            'command_publisher_remap',
+            default_value='/command',
+        ),
         Node(
             package='rosplane',
             executable='rosplane_controller',
             name='autopilot',
-            parameters = [autopilot_params],
-            output = 'screen',
-            arguments = [control_type]
+            parameters=[autopilot_params],
+            output='screen',
+            arguments=[control_type],
+            remappings=[
+                ('/command', launch.substitutions.LaunchConfiguration('command_publisher_remap'))
+            ]
+        ),
+        launch.actions.DeclareLaunchArgument(
+            'controller_command_publisher_remap',
+            default_value='/controller_command',
         ),
         Node(
             package='rosplane',
             executable='rosplane_path_follower',
             name='path_follower',
-            parameters = [autopilot_params],
+            parameters=[autopilot_params],
+            remappings=[
+                ('/controller_command', launch.substitutions.LaunchConfiguration('controller_command_publisher_remap'))
+            ]
         ),
         Node(
             package='rosplane',
             executable='rosplane_path_manager',
             name='path_manager',
-            parameters = [autopilot_params],
+            parameters=[autopilot_params],
         ),
         Node(
             package='rosplane',

diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp
@@ -18,7 +18,7 @@ controller_base::controller_base()
 
   // Advertise subscribed topics and set bound callbacks.
   controller_commands_sub_ = this->create_subscription<rosplane_msgs::msg::ControllerCommands>(
-    "controller_commands", 10, std::bind(&controller_base::controller_commands_callback, this, _1));
+    "controller_command", 10, std::bind(&controller_base::controller_commands_callback, this, _1));
   vehicle_state_sub_ = this->create_subscription<rosplane_msgs::msg::State>(
     "estimated_state", 10, std::bind(&controller_base::vehicle_state_callback, this, _1));
 

diff --git a/rosplane/src/param_manager.cpp → rosplane/src/param_manager/param_manager.cpp b/rosplane/src/param_manager.cpp → rosplane/src/param_manager/param_manager.cpp
diff --git a/rosplane/src/path_follower_base.cpp b/rosplane/src/path_follower_base.cpp
@@ -15,7 +15,7 @@ path_follower_base::path_follower_base()
     std::bind(&path_follower_base::current_path_callback, this, _1)); // the 1 may need to be 100
 
   controller_commands_pub_ =
-    this->create_publisher<rosplane_msgs::msg::ControllerCommands>("controller_commands", 1);
+    this->create_publisher<rosplane_msgs::msg::ControllerCommands>("controller_command", 1);
 
   parameter_callback_handle_ = this->add_on_set_parameters_callback(
     std::bind(&path_follower_base::parametersCallback, this, std::placeholders::_1));

diff --git a/rosplane_extra/CHANGELOG.rst b/rosplane_extra/CHANGELOG.rst
@@ -0,0 +1,8 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rosplane_extra
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.0.0 (Pre-release)
+------------------
+* Created package
+* Added input mapper
diff --git a/rosplane_extra/CMakeLists.txt b/rosplane_extra/CMakeLists.txt
@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.8)
+project(rosplane_extra)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE "Release")
+endif(NOT CMAKE_BUILD_TYPE)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(rosplane REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(rosplane_msgs REQUIRED)
+find_package(rosflight_msgs REQUIRED)
+
+include_directories(include)
+
+# Input Mapper
+add_executable(input_mapper
+  src/input_mapper.cpp)
+ament_target_dependencies(input_mapper rosplane_msgs rosflight_msgs rclcpp std_srvs rosplane)
+install(
+  TARGETS
+  input_mapper
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Launch files
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()