pretty much working teleop_ack code

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(TODO_PACKAGE_NAME)
+project(teleop_ack_joy)
 include(FetchContent)
 
 if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -9,40 +9,32 @@ endif ()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
-# Messages TODO_EXTRA
-find_package(ackermann_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-# OpenCV TODO_EXTRA
-find_package(cv_bridge REQUIRED)
-find_package(OpenCV 4.2.0 REQUIRED)
+find_package(joy REQUIRED)
+find_package(ackermann_msgs REQUIRED)
 
 # Add source for node executable (link non-ros dependencies here)
-add_executable(TODO_PACKAGE_NAME src/TODO_NODE_NAME.cpp src/TODO_NODE_NAME_node.cpp)
-target_include_directories(TODO_PACKAGE_NAME PUBLIC
+add_executable(teleop_ack_joy src/TeleopAckJoyNode.cpp src/TeleopAckJoyNode_node.cpp)
+target_include_directories(teleop_ack_joy PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>)
-target_compile_features(TODO_PACKAGE_NAME PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_compile_features(teleop_ack_joy PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 
 # Make ros deps a variable so they get linked to tests as well
 set(dependencies
         rclcpp
-        # Messages TODO_EXTRA
-        ackermann_msgs
+        joy
         sensor_msgs
-        std_msgs
-        # OpenCv TODO_EXTRA
-        cv_bridge
-        OpenCV
+        ackermann_msgs
         )
 
 # Link ros dependencies
 ament_target_dependencies(
-        TODO_PACKAGE_NAME
+        teleop_ack_joy
         ${dependencies}
 )
 
-install(TARGETS TODO_PACKAGE_NAME
+install(TARGETS teleop_ack_joy
         DESTINATION lib/${PROJECT_NAME})
 
 # Uncomment below to make launch files available if created
@@ -62,7 +54,7 @@ if (BUILD_TESTING)
     ament_add_gtest(${PROJECT_NAME}-test
             tests/unit.cpp
             # Remember to add node source files
-            src/TODO_NODE_NAME_node.cpp
+            src/TeleopAckJoyNode_node.cpp
             )
     ament_target_dependencies(${PROJECT_NAME}-test ${dependencies})
     target_include_directories(${PROJECT_NAME}-test PUBLIC

include/teleop_ack_joy/TeleopAckJoyNode_node.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "ackermann_msgs/msg/ackermann_drive.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/joy.hpp"
+
+class TeleopAckJoyNode : public rclcpp::Node {
+private:
+    // Sub
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub;
+
+    // Pub
+    rclcpp::Publisher<ackermann_msgs::msg::AckermannDrive>::SharedPtr ackermann_pub;
+
+public:
+    TeleopAckJoyNode(const rclcpp::NodeOptions& options);
+
+    float map_input(float in, float inMin, float inMax, float outMin, float outMax);
+
+    void joy_cb(sensor_msgs::msg::Joy::SharedPtr outputs);
+};

package.xml

@@ -1,7 +1,7 @@
 <?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>TODO_PACKAGE_NAME</name>
+    <name>teleop_ack_joy</name>
     <version>0.1.0</version>
     <description>A node template</description>
     <maintainer email="avealov@umich.edu">Andrew Ealovega</maintainer>
@@ -10,15 +10,9 @@
     <buildtool_depend>ament_cmake</buildtool_depend>
 
     <depend>rclcpp</depend>
-
-    <!--Messages TODO_EXTRA-->
-    <depend>ackermann_msgs</depend>
     <depend>sensor_msgs</depend>
-    <depend>std_msgs</depend>
-
-    <!--OpenCv Things TODO_EXTRA-->
-    <depend>cv_bridge</depend>
-    <depend>libopencv-dev</depend>
+    <depend>joy</depend>
+    <depend>ackermann_msgs</depend>
 
     <test_depend>ament_lint_auto</test_depend>
     <test_depend>ament_cmake_flake8</test_depend>
@@ -33,4 +27,4 @@
     <export>
         <build_type>ament_cmake</build_type>
     </export>
-</package>
+</package>

src/TODO_NODE_NAME.cpp → src/TeleopAckJoyNode.cpp

@@ -1,4 +1,4 @@
-#include "TODO_PACKAGE_NAME/TODO_NODE_NAME_node.hpp"
+#include "teleop_ack_joy/TeleopAckJoyNode_node.hpp"
 
 int main(int argc, char** argv) {
     // Setup runtime
@@ -7,7 +7,7 @@ int main(int argc, char** argv) {
     rclcpp::NodeOptions options;
 
     // Add nodes to executor
-    auto node = std::make_shared<TODO_NODE_NAME>(options);
+    auto node = std::make_shared<TeleopAckJoyNode>(options);
     exec.add_node(node);
 
     // Run

src/TeleopAckJoyNode_node.cpp

@@ -0,0 +1,30 @@
+#include "teleop_ack_joy/TeleopAckJoyNode_node.hpp"
+
+// For _1
+using namespace std::placeholders;
+
+TeleopAckJoyNode::TeleopAckJoyNode(const rclcpp::NodeOptions& options) : Node("TeleopAckjoyNode", options) {
+    joy_sub =
+        this->create_subscription<sensor_msgs::msg::Joy>("/joy", 5, std::bind(&TeleopAckJoyNode::joy_cb, this, _1));
+
+    ackermann_pub = this->create_publisher<ackermann_msgs::msg::AckermannDrive>("/nav_ack_vel", 1);
+}
+
+void TeleopAckJoyNode::joy_cb(sensor_msgs::msg::Joy::SharedPtr outputs) {
+    ackermann_msgs::msg::AckermannDrive command;
+
+    float left_stick_lr_val = outputs->axes.at(0);
+    float right_trigger_val = -outputs->axes.at(5);
+
+    command.steering_angle = map_input(left_stick_lr_val, -1, 1, -0.40, 0.40);
+    command.speed = map_input(right_trigger_val, -1, 1, 0, 5);
+
+    ackermann_pub->publish(command);
+}
+
+float TeleopAckJoyNode::map_input(float in, float inMin, float inMax, float outMin, float outMax) {
+    float slope = (outMax - outMin) / (inMax - inMin);
+    float b = outMax - slope * inMax;
+
+    return slope * in + b;
+}

tests/unit.cpp

@@ -2,7 +2,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "TODO_PACKAGE_NAME/TODO_NODE_NAME_node.hpp"
+#include "teleop_ack_joy/TeleopAckJoyNode_node.hpp"
 
 TEST(TODO_NODE_NAME, Test1) {}