From 52352f754ab0c3b869d9a02897f7c4f7be668d8a Mon Sep 17 00:00:00 2001 From: SW/FW Laptop Date: Sun, 30 Jun 2024 10:06:41 -0400 Subject: [PATCH] fix for coordinate nodes --- .../launch/location_launch.py | 11 +++++++---- .../src/coordinateNode.cpp | 14 ++++++++++---- .../src/uwrt_mars_rover_xbox_controller.cpp | 2 +- 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/launch/location_launch.py b/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/launch/location_launch.py index 4ec5f448..1018aaef 100644 --- a/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/launch/location_launch.py +++ b/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/launch/location_launch.py @@ -1,10 +1,12 @@ +import os import launch from launch import LaunchDescription -from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode +from launch.actions import SetEnvironmentVariable from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode def generate_launch_description(): + container = ComposableNodeContainer( name='container', namespace="", @@ -16,7 +18,8 @@ def generate_launch_description(): plugin='drivetraincontrollerComposition::CoordinateNode', name='coordinateNode' ), - ] + ], + output='screen' ) + return launch.LaunchDescription([container]) - diff --git a/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/src/coordinateNode.cpp b/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/src/coordinateNode.cpp index 5b3c87de..9c84a903 100644 --- a/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/src/coordinateNode.cpp +++ b/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/src/coordinateNode.cpp @@ -5,11 +5,17 @@ namespace drivetraincontrollerComposition CoordinateNode::CoordinateNode(const rclcpp::NodeOptions & options) : Node("coordinateNode", options) { auto callback = [this](const uwrt_mars_rover_xbox_controller::msg::XboxController::SharedPtr msg_in) -> void { //Add scaling value - const int vConstant = 5; + int vConstant = 5; auto msg = std::make_unique(); - msg->linear.x = (*msg_in).drivetrain_joy_x * vConstant; - msg->angular.z = (*msg_in).gimble_joy_x * vConstant; + msg->linear.x = (*msg_in).drivetrain_joy_y * vConstant; + msg->angular.z = (*msg_in).drivetrain_joy_x * vConstant; + msg->linear.x = (*msg_in).gimble_joy_y*(vConstant++); + msg->linear.x = (*msg_in).gimble_joy_x*(vConstant--); + + //To avoid overflow + if(vConstant > 2147483647 || vConstant < 0) + vConstant = 5; pub_->publish(std::move(msg)); }; @@ -21,4 +27,4 @@ CoordinateNode::CoordinateNode(const rclcpp::NodeOptions & options) : Node("coor } // namespace drivetraincontrollerComposition #include -RCLCPP_COMPONENTS_REGISTER_NODE(drivetraincontrollerComposition::CoordinateNode) +RCLCPP_COMPONENTS_REGISTER_NODE(drivetraincontrollerComposition::CoordinateNode) \ No newline at end of file diff --git a/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/src/uwrt_mars_rover_xbox_controller.cpp b/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/src/uwrt_mars_rover_xbox_controller.cpp index 7ee22193..d0405926 100644 --- a/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/src/uwrt_mars_rover_xbox_controller.cpp +++ b/uwrt_mars_rover_utils/uwrt_mars_rover_xbox_controller/src/uwrt_mars_rover_xbox_controller.cpp @@ -12,7 +12,7 @@ UWRTXboxController::UWRTXboxController(const rclcpp::NodeOptions & options) "joy", 10, std::bind(&UWRTXboxController::getXboxData, this, std::placeholders::_1)); xbox_node_pub = create_publisher("/xbox_info", 10); pub_timer = - create_wall_timer(100ms, std::bind(&UWRTXboxController::publishStructuredXboxData, this)); + create_wall_timer(1000ms, std::bind(&UWRTXboxController::publishStructuredXboxData, this)); } void UWRTXboxController::getXboxData(const joy_msg::SharedPtr msg)