fix merge issues

.github/ISSUE_TEMPLATE/config.yaml

@@ -0,0 +1 @@
+blank_issues_enabled: true

.github/ISSUE_TEMPLATE/dv-tasking-issue.md

@@ -0,0 +1,22 @@
+---
+name: DV Tasking Issue
+about: Issue template for subteam tasking
+title: "[SUBTEAM]-[Project name]"
+labels: ''
+assignees: ''
+
+---
+
+## Task Description:
+...
+
+## Task Check-list:
+- [ ] ...
+- [ ] Write integration tests
+- [ ] Add Documentation (to Confluence too)
+- [ ] Open Pull Request
+
+## Task Timeline:
+- [ ] Update [Jira](cmr.red/jira) with Task Timeline
+
+Comment any questions and tag reviewers.

.github/pull_request_template.md

@@ -0,0 +1,11 @@
+# Pull Request
+Addresses Issue #__ ([issue name])
+
+### Description
+Explain any proposed changes here. 
+
+### Check-list
+- [ ] Integration tests pass
+- [ ] Code Documented (and Confluence documentation updated)
+- [ ] Reviewer 1 approved
+

.github/workflows/fsdv-integration.yml

@@ -0,0 +1,45 @@
+name: cmr-fsdv
+
+on:
+  push:
+    branches: [ main ]
+  pull_request:
+    branches: [ main ]
+
+jobs:
+  build:
+
+    runs-on: ubuntu-20.04
+    strategy:
+      fail-fast: false
+      matrix:
+        python-version: ["3.9"]
+
+    steps:
+    - uses: actions/checkout@v3
+    - name: Set up Python ${{ matrix.python-version }}
+      uses: actions/setup-python@v3
+      with:
+        python-version: ${{ matrix.python-version }}
+
+    - uses: actions/cache@v2
+      with:
+        path: ${{ env.pythonLocation }}
+        key: ${{ env.pythonLocation }}-${{ hashFiles('setup.py') }}-${{ hashFiles('requirements.txt') }}
+
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        python -m pip install flake8 pytest
+        pip install --upgrade --upgrade-strategy eager -r requirements.txt
+    #   if [ -f requirements.txt ]; then pip install -r requirements.txt; fi
+
+    # - name: Lint with flake8
+    #   run: |
+    #     # stop the build if there are Python syntax errors or undefined names
+    #     flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
+    #     # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide
+    #     flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics
+    - name: Test with pytest
+      run: |
+        pytest

driverless_ws/src/planning/CMakeLists.txt

@@ -17,11 +17,27 @@ find_package(geometry_msgs REQUIRED)
 find_package(eufs_msgs REQUIRED)
 find_package(GSL REQUIRED)
 
-add_executable(midpoint_test src/midpoint.cpp src/generator.cpp src/raceline/raceline.cpp)
-ament_target_dependencies(midpoint_test rclcpp std_msgs eufs_msgs geometry_msgs GSL)
+add_executable(midline src/midpoint.cpp src/generator.cpp src/raceline/raceline.cpp)
+ament_target_dependencies(midline rclcpp std_msgs eufs_msgs geometry_msgs GSL)
 
+
+
+add_executable(talker pub.cpp)
+ament_target_dependencies(talker rclcpp std_msgs)
+
+install(TARGETS
+  talker
+  DESTINATION lib/${PROJECT_NAME})
+
+add_executable(listener sub.cpp)
+ament_target_dependencies(listener rclcpp std_msgs)
+
+install(TARGETS
+  talker
+  listener
+  DESTINATION lib/${PROJECT_NAME})
 install(TARGETS
-  midpoint_test
+  midline
   DESTINATION lib/${PROJECT_NAME})
 
 ament_package()

driverless_ws/src/planning/pub.cpp

@@ -0,0 +1,44 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+/* This example creates a subclass of Node and uses std::bind() to register a
+* member function as a callback from the timer. */
+
+class MinimalPublisher : public rclcpp::Node
+{
+  public:
+    MinimalPublisher()
+    : Node("minimal_publisher"), count_(0)
+    {
+      publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
+      timer_ = this->create_wall_timer(
+      500ms, std::bind(&MinimalPublisher::timer_callback, this));
+    }
+
+  private:
+    void timer_callback()
+    {
+      auto message = std_msgs::msg::String();
+      message.data = "Hello, world! " + std::to_string(count_++);
+      RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
+      publisher_->publish(message);
+    }
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+    size_t count_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<MinimalPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}

driverless_ws/src/planning/src/midpoint.cpp

@@ -20,13 +20,39 @@ struct raceline_pt{
 
 class MidpointNode : public rclcpp::Node
 {
-  private:
-    perceptionsData perception_data;
+
+  public:
+    MidpointNode()
+    : Node("midpoint")
+    {
+
+      RCLCPP_INFO(this->get_logger(), "Started Node");
+
+      subscription_ = this->create_subscription<std_msgs::msg::String>(
+      "topic", 10, std::bind(&MidpointNode::topic_callback, this, _1));
+
+      subscription_cones = this->create_subscription<eufs_msgs::msg::ConeArray>(
+        "/stereo_cones", 10, std::bind(&MidpointNode::cones_callback, this, _1));
+      // subscription_cones.subscribe(this,"/stereo_cones"); //= this->create_subscription<eufs_msgs::msg::ConeArray>("/stereo_cones", 10, std::bind(&MidpointNode::cones_callback, this, _1));
+
+      // subscription_lap_num = this->create_subscription<std_msgs::msg::String>("/lap_num", 10, std::bind(&MidpointNode::lap_callback, this, _1));
+      publisher_rcl_pt = this->create_publisher<eufs_msgs::msg::PointArray>("/midpoint_points",10);
+      //     rclcpp::TimerBase::SharedPtr  timer_ = this->create_wall_timer(
+      // 500ms, std::bind(&MinimalPublisher::timer_callback, this));
+      generator_mid = MidpointGenerator(10);
+      // generator_left = MidpointGenerator(10);
+      // generator_right = MidpointGenerator(10);
+      // VIS LOOKAHEADS 
+      RCLCPP_INFO(this->get_logger(), "Created Node");
 
-    // Normal cone array for pipeline
-    // rclcpp::Subscription<eufs_msgs::msg::ConeArray>::SharedPtr subscription_cones;
-    rclcpp::Subscription<eufs_msgs::msg::ConeArrayWithCovariance>::SharedPtr subscription_cones;
+    }
 
+    private:
+    perceptionsData perception_data;
+
+
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+    rclcpp::Subscription<eufs_msgs::msg::ConeArray>::SharedPtr subscription_cones;
     // rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_lap_num;
     rclcpp::Publisher<eufs_msgs::msg::PointArray>::SharedPtr publisher_rcl_pt;
 
@@ -36,23 +62,33 @@ class MidpointNode : public rclcpp::Node
     int lap = 1;
     bool vis_spline = true;
     MidpointGenerator generator_mid;
-    MidpointGenerator generator_left;
-    MidpointGenerator generator_right;
+    // MidpointGenerator generator_left;
+    // MidpointGenerator generator_right;
 
-    void lap_callback(const std_msgs::msg::Int8::SharedPtr msg) 
+    // void lap_callback(const std_msgs::msg::Int8::SharedPtr msg) 
+    // {
+    //   lap=msg->data;
+    // }
+
+    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
     {
-      lap=msg->data;
+      RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
     }
 
-    void cones_callback (const eufs_msgs::msg::ConeArray::SharedPtr msg)
+    void cones_callback(const eufs_msgs::msg::ConeArray::SharedPtr msg) const
     { 
+      RCLCPP_INFO(this->get_logger(), "Recieved cones from perceptions");
+    }
+
+    void cones_callback2(const eufs_msgs::msg::ConeArray::SharedPtr msg) 
+    { 
+      RCLCPP_INFO(this->get_logger(), "Recieved cones from perceptions");
       if (lap>1) return;
 
       if((msg->blue_cones.size()==0 || msg->yellow_cones.size()==0) && (msg->orange_cones.size()<2)){
         return;
       }
 
-
       for (auto e : msg->blue_cones)
       {
         perception_data.bluecones.emplace_back(e.x, e.y);

driverless_ws/src/planning/src/raceline/raceline.cpp

@@ -281,7 +281,7 @@ gsl_vector *get_translation_vector(gsl_matrix *group){
 
 gsl_matrix *transform_points(gsl_matrix *points, gsl_matrix *Q, gsl_vector *get_translation_vector){
     gsl_matrix *temp = gsl_matrix_alloc(points->size1,points->size2);
-    for(int i=0;i<temp->size2;++i){
+    for(u_int32_t i=0;i<temp->size2;++i){
         gsl_matrix_set(temp,0,i,gsl_matrix_get(points,0,i)-gsl_vector_get(get_translation_vector,0));
         gsl_matrix_set(temp,1,i,gsl_matrix_get(points,1,i)-gsl_vector_get(get_translation_vector,1));
     }
@@ -298,7 +298,7 @@ gsl_matrix *transform_points(gsl_matrix *points, gsl_matrix *Q, gsl_vector *get_
 
 gsl_matrix *reverse_transform(gsl_matrix *points, gsl_matrix *Q, gsl_vector *get_translation_vector){
     gsl_matrix *temp = gsl_matrix_alloc(points->size1,points->size2);
-    for(int i=0;i<temp->size2;++i){
+    for(uint32_t i=0;i<temp->size2;++i){
         gsl_matrix_set(temp,0,i,gsl_matrix_get(points,0,i));
         gsl_matrix_set(temp,1,i,gsl_matrix_get(points,1,i));
     }
@@ -307,7 +307,7 @@ gsl_matrix *reverse_transform(gsl_matrix *points, gsl_matrix *Q, gsl_vector *get
     gsl_linalg_matmult(temp,Q,ret);
     gsl_matrix_free(temp);    
 
-    for(int i=0;i<temp->size2;++i){
+    for(uint32_t i=0;i<temp->size2;++i){
         gsl_matrix_set(temp,0,i,gsl_matrix_get(points,0,i)+gsl_vector_get(get_translation_vector,0));
         gsl_matrix_set(temp,1,i,gsl_matrix_get(points,1,i)+gsl_vector_get(get_translation_vector,1));
     }
@@ -318,21 +318,21 @@ gsl_matrix *reverse_transform(gsl_matrix *points, gsl_matrix *Q, gsl_vector *get
 polynomial lagrange_gen(gsl_matrix* points){
     polynomial lagrange_poly = poly(3);
 
-    for(int col = 0;col <points->size2;col++){
+    for(uint32_t col = 0;col <points->size2;col++){
 
 
     }
     double x[points->size2];
     double y[points->size2];
-    for(int i=0;i<points->size2;i++){
+    for(uint32_t i=0;i<points->size2;i++){
         x[i] = gsl_matrix_get(points,i,0);
         y[i] = gsl_matrix_get(points,i,1);
     }
 
 
-    for(int i=0;i<points->size2;i++){
+    for(uint32_t i=0;i<points->size2;i++){
         polynomial p = poly_one();
-        for(int j=0;j<points->size2;j++){
+        for(uint32_t j=0;j<points->size2;j++){
             if(j!=i){
                 polynomial pr =poly_root(x[j]);
                 polynomial q =poly_mult(p,pr);

driverless_ws/src/planning/sub.cpp

@@ -0,0 +1,31 @@
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+using std::placeholders::_1;
+
+class MinimalSubscriber : public rclcpp::Node
+{
+  public:
+    MinimalSubscriber()
+    : Node("minimal_subscriber")
+    {
+      subscription_ = this->create_subscription<std_msgs::msg::String>(
+      "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
+    }
+
+  private:
+    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
+    {
+      RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
+    }
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<MinimalSubscriber>());
+  rclcpp::shutdown();
+  return 0;
+}