Corey onboarding 4

.github/workflows/workflows.yml

@@ -5,10 +5,12 @@ on:
     branches:
       - master
       - develop
+      - onboarding
   pull_request:
     branches:
       - master
       - develop
+      - onboarding
   workflow_dispatch:
     inputs:
       debug_enabled:

.gitignore

@@ -18,5 +18,7 @@ log
 02_V5/ghost_pros/src/ghost_control/**
 02_V5/ghost_pros/src/ghost_util/**
 
+09_External/*deb
+
 # Auto-generated
-02_V5/ghost_pros/include/robot_config.hpp
+02_V5/ghost_pros/include/robot_config.hpp

03_ROS/ghost_motion_planner_core/CMakeLists.txt

@@ -91,16 +91,4 @@ install(
   DESTINATION include
 )
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
 ament_package()

03_ROS/rviz_animators/CMakeLists.txt

@@ -0,0 +1,90 @@
+
+cmake_minimum_required(VERSION 3.8)
+project(rviz_animators)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+#find_package(ghost_msgs REQUIRED)
+
+
+set(DEPENDENCIES
+  ament_cmake
+  rclcpp
+  visualization_msgs
+)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+
+# if(BUILD_TESTING)
+#   find_package(ament_lint_auto REQUIRED)
+#   # the following line skips the linter which checks for copyrights
+#   # comment the line when a copyright and license is added to all source files
+#   set(ament_cmake_copyright_FOUND TRUE)
+#   # the following line skips cpplint (only works in a git repo)
+#   # comment the line when this package is in a git repo and when
+#   # a copyright and license is added to all source files
+#   set(ament_cmake_cpplint_FOUND TRUE)
+#   ament_lint_auto_find_test_dependencies()
+# endif()
+
+
+ament_export_dependencies(${DEPENDENCIES})
+
+set(INCLUDE
+  include
+)
+include_directories(${INCLUDE})
+ament_export_include_directories(${INCLUDE})
+
+
+add_executable(publisher src/publisher.cpp)
+ament_target_dependencies(publisher ${DEPENDENCIES})
+install(TARGETS
+  publisher
+  DESTINATION lib/${PROJECT_NAME})
+
+  add_library(subscriber SHARED src/subscriber.cpp)
+  ament_target_dependencies(subscriber
+    ${DEPENDENCIES}
+  )
+  ament_export_targets(subscriber HAS_LIBRARY_TARGET)
+  install(
+    TARGETS subscriber
+    EXPORT subscriber
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin
+    INCLUDES DESTINATION include
+  )
+
+  add_executable(subscriber_main src/subscriber_main.cpp)
+  ament_target_dependencies(subscriber_main
+  ${DEPENDENCIES}
+)
+target_link_libraries(subscriber_main
+  subscriber
+)
+
+install(TARGETS
+  subscriber_main
+  DESTINATION lib/${PROJECT_NAME})
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch config
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()

03_ROS/rviz_animators/include/config/test.yaml

@@ -0,0 +1,3 @@
+subscriber_node:
+  ros__parameters:
+    test_param: "test"

03_ROS/rviz_animators/include/rviz_animators/subscriber.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <memory>
+#include "visualization_msgs/msg/marker_array.hpp"
+// #include "ghost_msgs/msg/labeled_vector.hpp"
+// #include "ghost_msgs/msg/labeled_vector_map.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+
+namespace rviz{
+    class ROSSubscriber : public rclcpp::Node {
+    public:
+    ROSSubscriber();
+    void topic_callback(const visualization_msgs::msg::MarkerArray::SharedPtr msg);
+    // void vector_topic_callback(const ghost_msgs::msg::LabeledVectorMap::SharedPtr msg);
+
+    private:
+    rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr subscription_;
+    // rclcpp::Subscription<ghost_msgs::msg::LabeledVectorMap>::SharedPtr vector_subscription_;
+    };
+}

03_ROS/rviz_animators/launch/pub_sub.launch.py

@@ -0,0 +1,32 @@
+import launch
+import launch_ros
+
+import os
+
+def generate_launch_description():
+
+    home_dir = os.path.expanduser('~')
+    rviz_animators_dir = os.path.join(home_dir, "VEXU_GHOST", "03_ROS", "rviz_animators")
+
+    # This contains all the parameters for our ROS nodes
+    ros_config_file = os.path.join(rviz_animators_dir, "config/test.yaml")
+
+    publisher_node = launch_ros.actions.Node(
+        name = "publisher_node",
+        package = 'rviz_animators',
+        # This name is specified in the CmakeLists.txt file of this package 
+        executable = 'publisher',
+    )
+
+    subscriber_node = launch_ros.actions.Node(
+        name='subscriber_node',
+        package= 'rviz_animators',
+        # This name is specified in the CmakeLists.txt file of this package 
+        executable='subscriber_main',
+        parameters=[ros_config_file]
+    )
+
+    return launch.LaunchDescription([
+        publisher_node,
+        subscriber_node
+    ])

03_ROS/rviz_animators/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rviz_animators</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ghost_msgs</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

03_ROS/rviz_animators/src/publisher.cpp

@@ -0,0 +1,83 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include "visualization_msgs/msg/marker_array.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+// #include <ghost_msgs/msg/labeled_vector.hpp>
+// #include <ghost_msgs/msg/labeled_vector_map.hpp>
+#include "rclcpp/rclcpp.hpp"
+#include <vector>
+
+using namespace std::chrono_literals;
+
+class ROSPublisher : public rclcpp::Node
+{
+public:
+    ROSPublisher() : Node("marker_publisher"),
+                     count_(0)
+    {
+        publisher_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("visualization_marker", 10);
+        timer_ = this->create_wall_timer(500ms, std::bind(&ROSPublisher::timer_callback, this));
+    }
+
+private:
+    void timer_callback()
+    {
+        auto marker_array_msg = std::make_shared<visualization_msgs::msg::MarkerArray>();
+        for (int i = 0; i < 10; i++)
+        {
+            visualization_msgs::msg::Marker marker;
+            createSphereMarker(i, marker); // Create and fill marker directly
+            marker_array_msg->markers.push_back(marker);
+        }
+        publisher_->publish(*marker_array_msg);
+    }
+
+    void createSphereMarker(int id, visualization_msgs::msg::Marker &marker)
+    {
+        marker.header.frame_id = "map";
+        marker.header.stamp = this->get_clock()->now();
+        marker.ns = "basic_shapes";
+        marker.id = id;
+        marker.type = visualization_msgs::msg::Marker::SPHERE;
+        marker.action = visualization_msgs::msg::Marker::ADD;
+
+        // Define a function of time for position calculation
+        //auto t = this->get_clock()->now(); // Current time in seconds
+                                           // Current time in seconds
+        double x = 0;                      // Example function: x = t * cos(t + id)
+        double y = 0;                      // Example function: y = t * sin(t + id)
+        double z = 0;                      // Example function: z = t
+
+        marker.pose.position.x = x;
+        marker.pose.position.y = y;
+        marker.pose.position.z = z;
+
+        marker.pose.orientation.x = 0.0;
+        marker.pose.orientation.y = 0.0;
+        marker.pose.orientation.z = 0.0;
+        marker.pose.orientation.w = 1.0;
+
+        marker.scale.x = 1.0;
+        marker.scale.y = 1.0;
+        marker.scale.z = 1.0;
+
+        marker.color.a = 1.0;
+        marker.color.r = 1.0;
+        marker.color.g = 0.0;
+        marker.color.b = 0.0;
+    }
+
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr publisher_;
+    int count_;
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<ROSPublisher>());
+    rclcpp::shutdown();
+    return 0;
+}

03_ROS/rviz_animators/src/subscriber.cpp

@@ -0,0 +1,52 @@
+#include "rviz_animators/subscriber.hpp"
+
+namespace rviz
+{
+
+    ROSSubscriber::ROSSubscriber() : Node("subscribers")
+    {
+
+        this->declare_parameter("test_param", "/estimation/pose");
+        std::string test_param = this->get_parameter("test_param").as_string();
+
+        std::cout << test_param << std::endl;
+
+        // Create a subscription to the "marker_topic" topic with a queue size of 10
+        subscription_ = this->create_subscription<visualization_msgs::msg::MarkerArray>(
+            "marker_topic", 10, std::bind(&ROSSubscriber::topic_callback, this, std::placeholders::_1));
+
+        // vector_subscription_ = this->create_subscription<ghost_msgs::msg::LabeledVectorMap>(
+        //     "vector_topic", 10, std::bind(&ROSSubscriber::vector_topic_callback, this, std::placeholders::_1));
+        // Uncomment the following lines if you want to subscribe to a trajectory topic
+        // trajectory_subscription_ = this->create_subscription<trajectory_msgs::msg::JointTrajectoryPoint>(
+        //     "trajectory_topic", 10, std::bind(&ROSSubscriber::trajectory_topic_callback, this, std::placeholders::_1));
+    }
+
+    void ROSSubscriber::topic_callback(const visualization_msgs::msg::MarkerArray::SharedPtr msg)
+    {
+        auto now = this->get_clock()->now();
+        auto diff = now - msg->markers[0].header.stamp;
+        // Log the X-coordinate of the first marker in the array
+        RCLCPP_INFO(this->get_logger(), "X-coord is: %f", msg->markers[0].pose.position.x);
+    }
+
+    // void ROSSubscriber::vector_topic_callback(const ghost_msgs::msg::LabeledVectorMap::SharedPtr msg)
+    // {
+    //     // plays message back in rviz
+    //     // auto now = this->get_clock()->now();
+    //     // auto diff = now - msg->markers[0].header.stamp;
+    //     // Log the X-coordinate of the first marker in the array
+    //     RCLCPP_INFO(this->get_logger(), "X-coord is: %f", msg->markers[0].pose.position.x);
+   // }
+    rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr subscription_;
+   // rclcpp::Subscription<ghost_msgs::msg::LabeledVector>::SharedPtr vector_subscription_;
+
+    // Uncomment the following function if you want to handle messages of type trajectory_msgs::msg::JointTrajectoryPoint
+    // void ROSSubscriber::topic_callback(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr msg) {
+    //     auto now = this->get_clock()->now();
+    //     auto diff = now - msg->header.stamp;
+    //     // Log the X-coordinate of the received trajectory point
+    //     RCLCPP_INFO(this->get_logger(), "X-coord is: %f", msg->positions[0]);
+    // }
+
+} // namespace rviz

03_ROS/rviz_animators/src/subscriber_main.cpp

@@ -0,0 +1,8 @@
+#include "rviz_animators/subscriber.hpp"
+
+int main(int argc, char * argv[]){
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rviz::ROSSubscriber>());
+	rclcpp::shutdown();
+	return 0;
+}