Jkeller/droan gl integration

common/ros_packages/airstack_common/CMakeLists.txt

@@ -24,6 +24,9 @@ ament_target_dependencies(
   ros2_helper
   rclcpp
   )
+install(TARGETS ros2_helper
+  DESTINATION lib/${PROJECT_NAME}
+  )
 install(
   TARGETS ros2_helper
   EXPORT ros2_helper

common/ros_packages/airstack_common/include/airstack_common/ros2_helper.hpp

@@ -13,8 +13,17 @@ namespace airstack {
 
   template <typename T>
   inline T get_param(rclcpp::Node* node, std::string name, T default_value, bool* set) = delete;
-
+  /*  
+  template <>
+  inline int get_param(rclcpp::Node* node, std::string name, int default_value, bool* set);
+  template <>
+  inline double get_param(rclcpp::Node* node, std::string name, double default_value, bool* set);
+  template <>
+  inline std::string get_param(rclcpp::Node* node, std::string name, std::string default_value, bool* set);
   template <>
+  inline bool get_param(rclcpp::Node* node, std::string name, bool default_value, bool* set);
+  */
+    template <>
   inline int get_param(rclcpp::Node* node, std::string name, int default_value, bool* set){
     try{
       node->declare_parameter(name, rclcpp::PARAMETER_INTEGER);
@@ -64,8 +73,8 @@ namespace airstack {
     if(set != NULL)
       *set = s;
     return param.as_bool();
-  }
-  
+  }  
+
   template <typename T>
   inline T get_param(std::shared_ptr<rclcpp::Node> node, std::string name, T default_value, bool* set){
     return get_param(node.get(), name, default_value, set);
@@ -89,52 +98,31 @@ namespace airstack {
     void* variable;
     rclcpp::ParameterType type;
   };
-  std::unordered_map<std::string, ParamInfo> dynamic_params;
-  rclcpp::Node* temp_node = NULL;
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle;
-
-  rcl_interfaces::msg::SetParametersResult on_param_change(const std::vector<rclcpp::Parameter> & params){
-    rcl_interfaces::msg::SetParametersResult result;
-    result.successful = true;
-
-    if(temp_node != NULL){
-      for (const auto & param : params) {
-	RCLCPP_INFO_STREAM(temp_node->get_logger(), param.get_name());
-
-	if(dynamic_params.count(param.get_name()) > 0){
-	  ParamInfo pi = dynamic_params[param.get_name()];
-
-	  if(pi.type == rclcpp::ParameterType::PARAMETER_INTEGER)
-	    *((int*)pi.variable) = param.as_int();
-	  else if(pi.type == rclcpp::ParameterType::PARAMETER_DOUBLE)
-	    *((double*)pi.variable) = param.as_double();
-	  else if(pi.type == rclcpp::ParameterType::PARAMETER_STRING)
-	    *((std::string*)pi.variable) = param.as_string();
-	  else if(pi.type == rclcpp::ParameterType::PARAMETER_BOOL)
-	    *((bool*)pi.variable) = param.as_bool();
-	}
-      }
-    }
-
-
-
-    return result;
-  }
 
+  struct DynamicParamInfo {
+    std::unordered_map<std::string, ParamInfo> dynamic_params;
+    rclcpp::Node* temp_node = NULL;
+    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle;
+  };
+
+  DynamicParamInfo* get_dynamic_param_info();
+
+  rcl_interfaces::msg::SetParametersResult on_param_change(const std::vector<rclcpp::Parameter> & params);
+
   template <typename T>
   inline void dynamic_param(rclcpp::Node* node, std::string name, T default_value, T* variable){
-    temp_node = node;
+    get_dynamic_param_info()->temp_node = node;
     //rclcpp::ParameterValue pv = node->declare_parameter(name, rclcpp::ParameterValue(default_value));
     //*variable = default_value;
     *variable = get_param(node, name, default_value);
 
     ParamInfo pi;
     pi.variable = (void*)variable;
     pi.type = node->get_parameter(name).get_type();//pv.get_type();
-    dynamic_params[name] = pi;
+    get_dynamic_param_info()->dynamic_params[name] = pi;
 
-    if(!param_callback_handle)
-      param_callback_handle = node->add_on_set_parameters_callback(&on_param_change);
+    if(!get_dynamic_param_info()->param_callback_handle)
+      get_dynamic_param_info()->param_callback_handle = node->add_on_set_parameters_callback(&on_param_change);
   }
 
   // services

common/ros_packages/airstack_common/include/airstack_common/vislib.hpp

@@ -39,18 +39,20 @@ namespace vis {
   private:
     std::string ns;
     int id;
+    int reserve_size;
 
     std::vector<Marker> markers;
 
   public:
-    MarkerArray(std::string ns="default");
+    MarkerArray(std::string ns="default", int reserve_size=10);
 
     Marker& add_text(std::string frame_id, rclcpp::Time stamp, std::string text, double x, double y, double z);
     Marker& add_line_list(std::string frame_id, rclcpp::Time stamp, float r, float g, float b, float a, float width, int lines, ...);
     Marker& add_line_strip(std::string frame_id, rclcpp::Time stamp, std::vector<PointColor> points, float width=0.1f);
     Marker& add_arrow(std::string frame_id, rclcpp::Time stamp, geometry_msgs::msg::Pose pose,
 		      float length=0.5f, float width=0.05f, float height=0.05f);
     Marker& add_sphere(std::string frame_id, rclcpp::Time stamp, float x, float y, float z, float radius=0.1f);
+    Marker& add_points(std::string frame_id, rclcpp::Time stamp);
 
     void overwrite();
     visualization_msgs::msg::MarkerArray get_marker_array();

common/ros_packages/airstack_common/src/ros2_helper.cpp

@@ -2,6 +2,38 @@
 
 namespace airstack {
 
+
 
+  static DynamicParamInfo dynamic_param_info;
+
+  DynamicParamInfo* get_dynamic_param_info(){
+    return &dynamic_param_info;
+  }
+
+  rcl_interfaces::msg::SetParametersResult on_param_change(const std::vector<rclcpp::Parameter> & params){
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+
+    if(get_dynamic_param_info()->temp_node != NULL){
+      for (const auto & param : params) {
+	RCLCPP_INFO_STREAM(get_dynamic_param_info()->temp_node->get_logger(), param.get_name());
+
+	if(get_dynamic_param_info()->dynamic_params.count(param.get_name()) > 0){
+	  ParamInfo pi = get_dynamic_param_info()->dynamic_params[param.get_name()];
+
+	  if(pi.type == rclcpp::ParameterType::PARAMETER_INTEGER)
+	    *((int*)pi.variable) = param.as_int();
+	  else if(pi.type == rclcpp::ParameterType::PARAMETER_DOUBLE)
+	    *((double*)pi.variable) = param.as_double();
+	  else if(pi.type == rclcpp::ParameterType::PARAMETER_STRING)
+	    *((std::string*)pi.variable) = param.as_string();
+	  else if(pi.type == rclcpp::ParameterType::PARAMETER_BOOL)
+	    *((bool*)pi.variable) = param.as_bool();
+	}
+      }
+    }
+
+    return result;
+  }
 
 }

common/ros_packages/airstack_common/src/vislib.cpp

@@ -103,9 +103,10 @@ namespace vis{
     return marker;
   }
 
-  MarkerArray::MarkerArray(std::string ns){
+  MarkerArray::MarkerArray(std::string ns, int reserve_size){
     this->ns = ns;
     id = 0;
+    markers.reserve(reserve_size);
   }
 
   Marker& MarkerArray::add_text(std::string frame_id, rclcpp::Time stamp, std::string text, double x, double y, double z){
@@ -200,6 +201,17 @@ namespace vis{
     return markers[marker.get_id()];
   }
 
+  Marker& MarkerArray::add_points(std::string frame_id, rclcpp::Time stamp){
+    Marker marker(frame_id, stamp, ns, id++, visualization_msgs::msg::Marker::POINTS);
+
+    if(marker.get_id() >= markers.size())
+      markers.push_back(marker);
+    else
+      markers[marker.get_id()] = marker;
+
+    return markers[marker.get_id()];
+  }
+
   visualization_msgs::msg::MarkerArray MarkerArray::get_marker_array(){
     visualization_msgs::msg::MarkerArray marker_array;
 

gcs/docker/Dockerfile.gcs

@@ -57,6 +57,10 @@ RUN USER=gcs && \
   mkdir -p /etc/fixuid && \
   printf "user: $USER\ngroup: $GROUP\n" > /etc/fixuid/config.yml
 
+RUN wget https://get.foxglove.dev/desktop/latest/foxglove-studio-latest-linux-amd64.deb
+RUN apt -o Acquire::AllowInsecureRepositories=true -o Acquire::AllowDowngradeToInsecureRepositories=true update && apt -o APT::Get::AllowUnauthenticated=true install -y ./foxglove-studio-latest-linux-amd64.deb
+RUN apt -o APT::Get::AllowUnauthenticated=true install -y ros-humble-foxglove-bridge
+
 # Cleanup. Prevent people accidentally doing git commits from within Docker
 RUN apt purge git -y && apt autoremove -y && apt clean -y && rm -rf /var/lib/apt/lists/*
 

gcs/docker/Foxglove/.gitignore

@@ -0,0 +1,3 @@
+*
+
+!.gitignore

gcs/docker/gcs-base-docker-compose.yaml

@@ -23,11 +23,13 @@ services:
             - capabilities: [gpu]
               count: 1
               driver: nvidia
+    runtime: nvidia
     entrypoint: ''
     environment:
       - AUTOLAUNCH=${AUTOLAUNCH:-false}
       - DISPLAY
       - QT_X11_NO_MITSHM=1
+      - NVIDIA_DRIVER_CAPABILITIES=all
     image: ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_gcs
     # Needed to display graphical applications
     # ipc: host
@@ -45,6 +47,7 @@ services:
       - ../../common/inputrc:/etc/inputrc:rw
       - ../../.devcontainer/gcs/launch.json:/home/gcs/AirStack/.vscode/launch.json:rw
       - ../../.devcontainer/gcs/tasks.json:/home/gcs/AirStack/.vscode/tasks.json:rw
+      - ./Foxglove:/home/gcs/.config/Foxglove:rw
       # autonomy stack stuff
       - ../..:/home/gcs/AirStack
       - ../../common/ros_packages:/home/gcs/AirStack/gcs/ros_ws/src/common:rw # common ROS packages

gcs/ros_ws/src/gcs_bringup/config/gcs.perspective

@@ -4,14 +4,14 @@
     "mainwindow": {
       "keys": {
         "geometry": {
-          "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000001ee0000007f000009bc000004be000001ee000000a4000009bc000004be00000000000000000a00000001ee000000a4000009bc000004be')",
+          "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000001ed00000086000009bb000004c5000001ed000000ab000009bb000004c500000000000000000a00000001ed000000ab000009bb000004c5')",
           "type": "repr(QByteArray.hex)",
           "pretty-print": "                                 "
         },
         "state": {
-          "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd0000000100000003000007cf000003f1fc0100000002fb0000006a007200710074005f00670072006f0075006e0064005f0063006f006e00740072006f006c005f00730074006100740069006f006e005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f0100000000000003e9000002cd00fffffffc000003ef000003e0000000d000fffffffa000000010200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000000ffffffff0000004a00fffffffb0000006a007200710074005f0061006900720073007400610063006b005f0063006f006e00740072006f006c005f00700061006e0065006c005f005f0041006900720073007400610063006b0043006f006e00740072006f006c00500061006e0065006c005f005f0031005f005f0100000000ffffffff0000036300ffffff000007cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+          "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd0000000100000003000007cf000003f1fc0100000003fb0000006a007200710074005f00670072006f0075006e0064005f0063006f006e00740072006f006c005f00730074006100740069006f006e005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f0100000000000003e90000000000000000fc0000000000000428000000d000fffffffa000000010200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000000ffffffff0000004a00fffffffb0000006a007200710074005f0061006900720073007400610063006b005f0063006f006e00740072006f006c005f00700061006e0065006c005f005f0041006900720073007400610063006b0043006f006e00740072006f006c00500061006e0065006c005f005f0031005f005f0100000000ffffffff0000036300fffffffb00000044007200710074005f006700630073005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f010000042e000003a10000036200ffffff000007cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
           "type": "repr(QByteArray.hex)",
-          "pretty-print": "                 jrqt_gcs__GroundControlStation__1__                                                               J                                                               c                                                          "
+          "pretty-print": "                 jrqt_ground_control_station__GroundControlStation__1__            (                                                  J                                                               c                                        .   b                                                          "
         }
       },
       "groups": {
@@ -57,7 +57,7 @@
             "plugin": {
               "keys": {
                 "info_dcts": {
-                  "repr": "[{'enable_display': True, 'excluded_services': [], 'hostname': '172.17.0.1', 'name': 'Localhost', 'namespace': 'none', 'password': '\\U000f01de\\U000f01db\\U000f01dc\\U000f01da', 'path': '~/temp_airstack/merge/airstack', 'username': 'john'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.11', 'name': 'NX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.12', 'name': 'AGX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.21', 'name': 'NX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.22', 'name': 'AGX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.31', 'name': 'NX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.32', 'name': 'AGX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}]",
+                  "repr": "[{'enable_display': True, 'excluded_services': [], 'hostname': '172.17.0.1', 'name': 'Localhost', 'namespace': 'none', 'password': '\\U000f01de\\U000f01db\\U000f01dc\\U000f01da', 'path': '~/airstack', 'username': 'john'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.11', 'name': 'NX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.12', 'name': 'AGX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.21', 'name': 'NX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.22', 'name': 'AGX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.31', 'name': 'NX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'gcs', 'gcs-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.32', 'name': 'AGX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}]",
                   "type": "repr"
                 }
               },

robot/docker/.bashrc

@@ -69,7 +69,7 @@ sws # source the ROS2 workspace by default
 container_name=$(host $(host $(hostname) | awk '{print $NF}') | awk '{print $NF}' | awk -F . '{print $1}')
 
 # remove the prefix and convert dashes to underscores
-export ROBOT_NAME=$(echo "$container_name" | sed 's/.*-\(robot-[0-9]*\)$/\1/' | sed 's#-#_#')
+export ROBOT_NAME=$(echo "$container_name" | sed 's/.*\(robot-[0-9]*\)$/\1/' | sed 's#-#_#')
 export ROS_DOMAIN_ID=$(echo "$ROBOT_NAME" | awk -F'_' '{print $NF}')
 
 # case: will be null on real world robot

robot/docker/docker-compose.yaml

@@ -7,7 +7,6 @@ services:
     profiles:
       - ""
       - sitl
-      - simple
     extends:
       file: ./robot-base-docker-compose.yaml
       service: robot_base
@@ -22,6 +21,7 @@ services:
     # we use tmux sd-keys so that the session stays alive
     environment:
       - AUTOLAUNCH=${AUTOLAUNCH:-true}
+      - NVIDIA_DRIVER_CAPABILITIES=all
     command: >
       bash -c "
       service ssh restart;
@@ -40,6 +40,16 @@ services:
     # for multiple robots
     deploy:
       replicas: ${NUM_ROBOTS:-1}
+    runtime: nvidia
+
+  simple-robot:
+    profiles:
+      - simple
+    extends:
+      file: ./docker-compose.yaml
+      service: robot
+    environment:
+      - SIM_TYPE=simple
 
   # ===================================================================================================================
   # for running on an NVIIDA jetson (linux for tegra) device

robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml

@@ -7,14 +7,14 @@
     <!-- Nodes -->
 
     <!-- Launch MAVROS (poll until connection is established) -->
-    <group>
+    <group unless="$(eval '\'$(env SIM_TYPE not_simple)\' == \'simple\'')">
         <push-ros-namespace namespace="interface" />
         <!-- <include file="$(find-pkg-share mavros_interface)/launch/mavros_connection_poll.launch.py"> -->
         <include file="$(find-pkg-share interface_bringup)/launch/px4.launch.xml">
             <!-- See robot .bashrc for how this gets set -->
             <arg name="fcu_url" value="$(env FCU_URL)" />
             <arg name="tgt_system" value="$(env TGT_SYSTEM)" />
-        </include>
+          </include>
     </group>
 
     <!-- Interface between AirStack and MAVROS -->