diff --git a/CMakeLists.txt b/CMakeLists.txt index 39e95cc..7b45012 100644 --- a/CMakeLists.txt +++ b/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 $ $) -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 diff --git a/include/TODO_PACKAGE_NAME/TODO_NODE_NAME_node.hpp b/include/TODO_PACKAGE_NAME/TODO_NODE_NAME_node.hpp deleted file mode 100644 index d8fe4d5..0000000 --- a/include/TODO_PACKAGE_NAME/TODO_NODE_NAME_node.hpp +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -class TODO_NODE_NAME : public rclcpp::Node { -private: - rclcpp::Publisher::SharedPtr pub; - - rclcpp::Subscription::SharedPtr sub; - -public: - TODO_NODE_NAME(const rclcpp::NodeOptions& options); - - /// subscriber callback - void sub_cb(std_msgs::msg::String::SharedPtr msg); -}; diff --git a/include/teleop_ack_joy/TeleopAckJoyNode_node.hpp b/include/teleop_ack_joy/TeleopAckJoyNode_node.hpp new file mode 100644 index 0000000..9906b7d --- /dev/null +++ b/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::SharedPtr joy_sub; + + // Pub + rclcpp::Publisher::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); +}; diff --git a/package.xml b/package.xml index 62544a7..506fe28 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ - TODO_PACKAGE_NAME + teleop_ack_joy 0.1.0 A node template Andrew Ealovega @@ -10,15 +10,9 @@ ament_cmake rclcpp - - - ackermann_msgs sensor_msgs - std_msgs - - - cv_bridge - libopencv-dev + joy + ackermann_msgs ament_lint_auto ament_cmake_flake8 @@ -33,4 +27,4 @@ ament_cmake - + \ No newline at end of file diff --git a/src/TODO_NODE_NAME_node.cpp b/src/TODO_NODE_NAME_node.cpp deleted file mode 100644 index 3f7bc6e..0000000 --- a/src/TODO_NODE_NAME_node.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "TODO_PACKAGE_NAME/TODO_NODE_NAME_node.hpp" - -// For _1 -using namespace std::placeholders; - -TODO_NODE_NAME::TODO_NODE_NAME(const rclcpp::NodeOptions& options) : Node("TODO_NODE_NAME", options) { - // Parameters - float x = this->declare_parameter("foo", -10.0); - - // Pub Sub - this->sub = - this->create_subscription("/str", 1, std::bind(&TODO_NODE_NAME::sub_cb, this, _1)); - this->pub = this->create_publisher("/run_folder", 1); - - // Log a sample log - RCLCPP_INFO(this->get_logger(), "You passed %f", x); - - // Send a sample message - std_msgs::msg::String msg{}; - msg.data = std::string{"Hello World!"}; - pub->publish(msg); -} - -void TODO_NODE_NAME::sub_cb(const std_msgs::msg::String::SharedPtr msg) { - // Echo message - this->pub->publish(*msg); -} diff --git a/src/TODO_NODE_NAME.cpp b/src/TeleopAckJoyNode.cpp similarity index 72% rename from src/TODO_NODE_NAME.cpp rename to src/TeleopAckJoyNode.cpp index 9ec0141..b9ff86e 100644 --- a/src/TODO_NODE_NAME.cpp +++ b/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(options); + auto node = std::make_shared(options); exec.add_node(node); // Run diff --git a/src/TeleopAckJoyNode_node.cpp b/src/TeleopAckJoyNode_node.cpp new file mode 100644 index 0000000..7ae3474 --- /dev/null +++ b/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("/joy", 5, std::bind(&TeleopAckJoyNode::joy_cb, this, _1)); + + ackermann_pub = this->create_publisher("/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; +} diff --git a/tests/unit.cpp b/tests/unit.cpp index e28e06d..6d9f60d 100644 --- a/tests/unit.cpp +++ b/tests/unit.cpp @@ -2,7 +2,7 @@ #include -#include "TODO_PACKAGE_NAME/TODO_NODE_NAME_node.hpp" +#include "teleop_ack_joy/TeleopAckJoyNode_node.hpp" TEST(TODO_NODE_NAME, Test1) {}