ROS2 Trapezoidal planner + mobility testing

.gitignore

@@ -89,6 +89,7 @@ astrobee/zones/*.bin
 /*-rootfs
 /*-kernel
 /*-logs
+/log
 /qped.sublime-workspace
 *.egg
 *.egg-info

astrobee/config/management/sys_monitor.config

@@ -65,7 +65,7 @@ nodelet_info={
                 {name="llp_disk_monitor", manager="llp_monitors", type="disk_monitor/DiskMonitor"},
                 {name="localization_manager", manager="mlp_localization", type="localization_manager/LocalizationManagerNodelet"},
                 {name="localization_node", manager="mlp_vision", type="localization_node/LocalizationNodelet"},
-                {name="mapper", manager="mlp_mapper", type="mapper/MapperNodelet"},
+                {name="mapper", manager="mlp_mapper", type="mapper/MapperComponent"},
                 {name="marker_tracking", manager="mlp_vision", type="marker_tracking_node/MarkerTrackingNodelet"},
                 {name="mlp_cpu_mem_monitor", manager="mlp_monitors", type="cpu_mem_monitor/CpuMemMonitor"},
                 {name="mlp_disk_monitor", manager="mlp_monitors", type="disk_monitor/DiskMonitor"},

astrobee/launch/robot/LLP.launch.py

@@ -53,20 +53,18 @@ def generate_launch_description():
         name='llp_gnc',
         namespace='',
         package='rclcpp_components',
-        executable='component_container',
+        executable='component_container_mt',
         composable_node_descriptions=[
             ComposableNode(
                 package='ctl',
                 plugin='ctl::CtlComponent',
                 name='ctl',
-                extra_arguments=[{'use_intra_process_comms': True}]
-                ),
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             ComposableNode(
                 package='fam',
                 plugin='fam::FamComponent',
                 name='fam',
-                extra_arguments=[{'use_intra_process_comms': True}]
-                )
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             ]
         ),
         ComposableNodeContainer(
@@ -195,7 +193,7 @@ def generate_launch_description():
                 package='light_flow',
                 plugin='light_flow::LightFlowComponent',
                 name='light_flow',
-                extra_arguments=[{'use_intra_process_comms': True}]),
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             ]
         ),
     ])

astrobee/launch/robot/MLP.launch

@@ -137,7 +137,7 @@
 
   <!-- Mobility -->
   <include file="$(find ff_util)/launch/ff_nodelet.launch">
-    <arg name="class" value="mapper/MapperNodelet" />
+    <arg name="class" value="mapper/MapperComponent" />
     <arg name="name" value="mapper" />
     <arg name="manager" value="mlp_mapper" />
     <arg name="spurn" value="$(arg spurn)" />

astrobee/launch/robot/MLP.launch.py

@@ -59,12 +59,12 @@ def generate_launch_description():
                  package='localization_manager',
                  plugin='localization_manager::LocalizationManagerComponent',
                  name='localization_manager',
-                ),
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             ComposableNode(
                 package='ground_truth_localizer',
                 plugin='ground_truth_localizer::GroundTruthLocalizerComponent',
-                name='ground_truth_localizer'
-                ),
+                name='ground_truth_localizer',
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             # ComposableNode(
             #     package='image_sampler',
             #     plugin='image_sampler::ImageSampler',
@@ -81,11 +81,11 @@ def generate_launch_description():
         executable='component_container',
         condition=UnlessCondition(LaunchConfiguration("gtloc")),
         composable_node_descriptions=[
-            # ComposableNode(
-            #     package='localization_manager',
-            #     plugin='localization_manager::LocalizationManagerNodelet',
-            #     name='localization_manager',
-            #     extra_arguments=[{'use_intra_process_comms': True}]),
+            ComposableNode(
+                package='localization_manager',
+                plugin='localization_manager::LocalizationManagerNodelet',
+                name='localization_manager',
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             # ComposableNode(
             #     package='image_sampler',
             #     plugin='image_sampler::ImageSampler',
@@ -222,11 +222,11 @@ def generate_launch_description():
         package='rclcpp_components',
         executable='component_container',
         composable_node_descriptions=[
-            # ComposableNode(
-            #     package='mapper',
-            #     plugin='mapper::MapperNodelet',
-            #     name='mapper',
-            #     extra_arguments=[{'use_intra_process_comms': True}]),
+            ComposableNode(
+                package='mapper',
+                plugin='mapper::MapperComponent',
+                name='mapper',
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             ]
         ),
         ComposableNodeContainer(
@@ -348,18 +348,17 @@ def generate_launch_description():
                 package='choreographer',
                 plugin='choreographer::ChoreographerComponent',
                 name='choreographer',
-                extra_arguments=[{'use_intra_process_comms': False}]),
-            # ComposableNode(
-            #     package='planner_trapezoidal',
-            #     plugin='planner_trapezoidal::PlannerTrapezoidalNodelet',
-            #     name='planner_trapezoidal',
-            #     extra_arguments=[{'use_intra_process_comms': True}]),
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
+            ComposableNode(
+                package='planner_trapezoidal',
+                plugin='planner_trapezoidal::PlannerTrapezoidalComponent',
+                name='planner_trapezoidal',
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             ComposableNode(
                 package='framestore',
                 plugin='mobility::FrameStore',
                 name='framestore',
-                # extra_arguments=[{'use_intra_process_comms': True}]
-                ),
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             ]
         ),
         ComposableNodeContainer(
@@ -424,7 +423,7 @@ def generate_launch_description():
                 package='states',
                 plugin='states::StatesComponent',
                 name='states',
-                extra_arguments=[{'use_intra_process_comms': True}]),
+                extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]),
             ]
         ),
     ])

communications/ff_msgs/action/Motion.action

@@ -111,7 +111,7 @@ ff_msgs/ControlState[] segment
 ff_msgs/MotionState state
 
 # Control progress
-# ff_msgs/ControlFeedback progress       # TODO(@mgouveia): ROS2 does not allow this
+ff_msgs/ControlFeedback progress
 
 
 # Planner progress

communications/ff_msgs/msg/ControlFeedback.msg

@@ -0,0 +1,28 @@
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+#
+# The state of the arm behavior
+
+
+uint32 index                                # Index being processed
+
+ff_msgs/ControlState setpoint               # Current setpoint
+
+float32 error_position                      # Position error
+float32 error_attitude                      # Attitude error
+float32 error_velocity                      # Velocity error
+float32 error_omega                         # Omega error

gnc/ctl/src/ctl_component.cc

@@ -30,7 +30,7 @@ class CtlComponent : public ff_util::FreeFlyerComponent {
   explicit CtlComponent(const rclcpp::NodeOptions & options) : ff_util::FreeFlyerComponent(options, NODE_CTL) {}
   ~CtlComponent() {}
   // This is called when the nodelet is loaded into the nodelet manager
-  void Initialize(NodeHandle nh) {
+  void Initialize(NodeHandle &nh) {
     // this used to be multi-threaded, but don't think it needs to be
     ctl_.reset(new ctl::Ctl(nh, GetName()));
   }

gnc/fam/src/fam_component.cc

@@ -30,7 +30,7 @@ class FamComponent : public ff_util::FreeFlyerComponent {
   explicit FamComponent(const rclcpp::NodeOptions & options) : ff_util::FreeFlyerComponent(options, NODE_FAM) {}
   ~FamComponent() {}
   // This is called when the nodelet is loaded into the nodelet manager
-  void Initialize(NodeHandle nh) {
+  void Initialize(NodeHandle &nh) {
     // this used to be multi-threaded, but don't think it needs to be
     fam_.reset(new fam::Fam(nh));
   }

gnc/pmc/include/pmc/shared.h

@@ -20,6 +20,7 @@
 #define PMC_SHARED_H_
 
 #include <Eigen/Dense>
+#include <limits>
 
 namespace pmc {
 
@@ -74,6 +75,24 @@ class PMCConstants {
 
 float Lookup(int table_size, const float lookup[], const float breakpoints[], float value);
 
+template <class MatT>
+Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> pseudoInverse(
+  const MatT& mat, typename MatT::Scalar tolerance = typename MatT::Scalar{std::numeric_limits<double>::epsilon()}) {
+  typedef typename MatT::Scalar Scalar;
+  auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
+  const auto& singularValues = svd.singularValues();
+  Eigen::Matrix<Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> singularValuesInv(mat.cols(), mat.rows());
+  singularValuesInv.setZero();
+  for (unsigned int i = 0; i < singularValues.size(); ++i) {
+    if (singularValues(i) > tolerance) {
+      singularValuesInv(i, i) = Scalar {1} / singularValues(i);
+    } else {
+      singularValuesInv(i, i) = Scalar {0};
+    }
+  }
+  return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint();
+}
+
 }  // end namespace pmc
 
 #endif  // PMC_SHARED_H_

gnc/pmc/src/fam.cc

@@ -121,9 +121,7 @@ void Fam::UpdateCOM(const Eigen::Vector3f & com) {
   thrust2force_ = -fam_nozzle_orientations.transpose();
   Eigen::Matrix<float, 6, 12> thrust2forcetorque;
   thrust2forcetorque << thrust2force_, thrust2torque_;
-  forcetorque2thrust_ = Eigen::MatrixXf::Identity(12, 6);
-  Eigen::JacobiSVD<Eigen::MatrixXf> svd(thrust2forcetorque, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  forcetorque2thrust_ = svd.solve(forcetorque2thrust_);
+  forcetorque2thrust_ = pseudoInverse(thrust2forcetorque);
 }
 
 void Fam::CalcThrustMatrices(const Eigen::Vector3f & force, const Eigen::Vector3f & torque,

mobility/choreographer/CMakeLists.txt

@@ -116,17 +116,27 @@ rclcpp_components_register_nodes(choreographer "choreographer::ChoreographerComp
 ## Install ##
 #############
 
+ament_export_dependencies(
+  config_reader
+  msg_conversions
+  ff_common
+  ff_util
+  jsonloader
+)
+
 ament_export_include_directories(include)
 
 # Mark libraries for installation
 install(TARGETS ${PROJECT_NAME}
+  EXPORT choreographer
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
 )
 
 # Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/
+install(DIRECTORY include/${PROJECT_NAME}
   DESTINATION include
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE