fix for coordinate nodes

uwrobotics · Jun 30, 2024 · 52352f7 · 52352f7
1 parent fa890e3
commit 52352f7
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 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
@@ -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
@@ -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<geometry_msgs::msg::Twist>();
-        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_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(drivetraincontrollerComposition::CoordinateNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(drivetraincontrollerComposition::CoordinateNode)
diff --git a/..._mars_rover_utils/uwrt_mars_rover_xbox_controller/src/uwrt_mars_rover_xbox_controller.cpp b/..._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_msg>("/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)