twist adjustments

src/dcaiti_navigation/config/slam.yml

@@ -2,5 +2,5 @@ slam_toolbox:
   ros__parameters:
     base_frame: base_link
     map_update_interval: 1.0
-    scan_topic: /scan
+    scan_topic: /scan_no_nan
     odom_topic: /diff_cont/odom

src/dcaiti_navigation/launch/start_nav2.py

@@ -11,6 +11,8 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetE
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import Command, LaunchConfiguration, PythonExpression
 from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node, SetRemap
+from launch.actions import GroupAction
 
 def generate_launch_description():
 
@@ -57,7 +59,7 @@ def generate_launch_description():
     
   declare_use_sim_time_cmd = DeclareLaunchArgument(
     name='use_sim_time',
-    default_value='True',
+    default_value='False',
     description='Use simulation (Gazebo) clock if true')
 
   slam_params = os.path.join(
@@ -67,10 +69,19 @@ def generate_launch_description():
         )
   
 
-  # Launch the ROS 2 Navigation Stack
+  start_ros2_navigation_cmd = GroupAction(
-  start_ros2_navigation_cmd = IncludeLaunchDescription(
+    actions=[
+        SetRemap(src='/cmd_vel',dst='/cmd_vel_nav'),
+        IncludeLaunchDescription(
     PythonLaunchDescriptionSource([os.path.join(
-    get_package_share_directory('nav2_bringup'),'launch','navigation_launch.py')]))
+    get_package_share_directory('nav2_bringup'),'launch','navigation_launch.py')]),
+    launch_arguments={
+                'use_sim_time': 'false',
+                '-r': '/cmd_vel:=/cmd_vel_nav'
+    }.items()
+    )
+    ]
+)
   
 
   
@@ -79,16 +90,33 @@ def generate_launch_description():
                 get_package_share_directory('slam_toolbox'),'launch','online_async_launch.py'
             )]), 
             launch_arguments={
-                'use_sim_time': 'true', 
+                'use_sim_time': 'false', 
                 'slam_params_file': slam_params}.items()
   )
 
+
+  node_fix_lidar = Node(
+        package="dcaitirobot",
+        executable="fixlidar",
+        name="fixlidar",
+    )
+
+  tfs1 = Node(package = "tf2_ros", 
+                      executable = "static_transform_publisher",
+                      arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "velodyne"])
+
+  tfs2 = Node(package = "tf2_ros", 
+                      executable = "static_transform_publisher",
+                      arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "base_footprint"])
+
+
+
   exploration = IncludeLaunchDescription(
             PythonLaunchDescriptionSource([os.path.join(
                 get_package_share_directory('explore_lite'),'launch','explore.launch.py'
             )]), 
             launch_arguments={
-                'use_sim_time': 'true'}.items()
+                'use_sim_time': 'false'}.items()
   )
 
   # Create the launch description and populate
@@ -101,6 +129,9 @@ def generate_launch_description():
       declare_use_sim_time_cmd,
       start_ros2_navigation_cmd,
       slam_toolbox,
+      node_fix_lidar,
+      tfs1,
+      tfs2,
       exploration,
   ])
 

src/dcaiti_navigation/params/nav2_params.yaml

@@ -1,6 +1,6 @@
 amcl:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     alpha1: 0.002
     alpha2: 0.002
     alpha3: 0.002
@@ -43,15 +43,15 @@ amcl:
 
 amcl_map_client:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 amcl_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 bt_navigator:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     global_frame: map
     robot_base_frame: base_link
     odom_topic: /diff_cont/odom #/wheel/odometry
@@ -99,11 +99,11 @@ bt_navigator:
 
 bt_navigator_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 controller_server:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     controller_frequency: 2.0
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
@@ -174,7 +174,7 @@ controller_server:
 
 controller_server_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 local_costmap:
   local_costmap:
@@ -183,7 +183,7 @@ local_costmap:
       publish_frequency: 2.0
       global_frame: odom
       robot_base_frame: base_link
-      use_sim_time: True
+      use_sim_time: False
       rolling_window: true
       width: 10
       height: 10
@@ -205,7 +205,7 @@ local_costmap:
         mark_threshold: 0
         observation_sources: scan
         scan:
-          topic: /scan
+          topic: /scan_no_nan
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -219,10 +219,10 @@ local_costmap:
       always_send_full_costmap: True
   local_costmap_client:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
   local_costmap_rclcpp_node:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
 
 global_costmap:
   global_costmap:
@@ -231,7 +231,7 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      use_sim_time: True
+      use_sim_time: False
       robot_radius: 0.21
       resolution: 0.05
       track_unknown_space: true
@@ -241,7 +241,7 @@ global_costmap:
         enabled: True
         observation_sources: scan
         scan:
-          topic: /scan
+          topic: /scan_no_nan
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -260,19 +260,19 @@ global_costmap:
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
   global_costmap_rclcpp_node:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
 
 map_server:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     yaml_filename: "turtlebot3_world.yaml"
 
 map_saver:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     save_map_timeout: 5.0
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
@@ -281,7 +281,7 @@ map_saver:
 planner_server:
   ros__parameters:
     expected_planner_frequency: 2.0
-    use_sim_time: True
+    use_sim_time: False
     planner_plugins: ["GridBased"]
     GridBased:
       plugin: "nav2_navfn_planner/NavfnPlanner"
@@ -291,7 +291,7 @@ planner_server:
 
 planner_server_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 recoveries_server:
   ros__parameters:
@@ -308,7 +308,7 @@ recoveries_server:
     global_frame: odom
     robot_base_frame: base_link
     transform_timeout: 0.1
-    use_sim_time: true
+    use_sim_time: False
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
     min_rotational_vel: 0.4
@@ -316,7 +316,7 @@ recoveries_server:
 
 robot_state_publisher:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     
 waypoint_follower:
   ros__parameters:
@@ -341,8 +341,8 @@ slam_toolbox:
 
     base_frame: base_link
     map_update_interval: 0.1
-    scan_topic: /scan
+    scan_topic: /scan_no_nan
-    odom_topic: /diff_cont/odom
+    odom_topic: /odom
 
     # if you'd like to immediately start continuing a map at a given pose
     # or at the dock, but they are mutually exclusive, if pose is given

src/dcaitirobot/dcaitirobot/joy.py

@@ -207,53 +207,11 @@ class JoystickController(Joystick):
             self.__axis_update()
 
     def __button_update(self):
-        print(
+        pass
-            "button: {} \tvalue: {}".format(
-                self.update_id, self.getButtonState(self.update_id)
-            )
-        )
 
     def __axis_update(self):
-        """Uses the left and right joysticks to produce a direction vector and a rotation vector.
+        pass
-        On a traditional mecanum robot, it can locomote in all directions with no rotation. This is
+   
-        why they are separate. The vectors are in polar coordinates.
-        """
-        print(f"{self.update_id} {self.getAxisState(self.update_id)}")
-        # x = self.getAxisState(JSEvent.AXIS_LEFT_STICK_HORIZONTAL)
-        # # down and right is positive, so flip y axis
-        # y = -self.getAxisState(JSEvent.AXIS_LEFT_STICK_VERTICAL)
-
-        # # TODO: define SHRT_MAX (32767) as as large as the radius can get, so scale appropriately?
-        # # interestingly enough, we don't have a circular portion of the plane to work with, we can
-        # # get the largest vectors in the 45 degree direction. Think about how this will effect
-        # # scaling the vectors' magnitude
-
-        # # TODO: Think about headless - rotation being a difference from the current direction
-        # # or actually what it would take... This is actually a part of the *robots* locomotion
-        # # code, not the joystick. Do headless stuff there.
-        # # by headless I mean - there is no sense of front - the current direction of locomotion is
-        # # forwards.
-
-        # # TODO: Think about what kind of coordinate transformations would be useful...
-        # d = self.__to_polar(x, y)
-
-        # rotate_x = self.getAxisState(JSEvent.AXIS_RIGHT_STICK_HORIZONTAL)
-        # rotate_y = -self.getAxisState(JSEvent.AXIS_RIGHT_STICK_VERTICAL)
-        # r = self.__to_polar(rotate_x, rotate_y)
-
-        # self.direction_vector = d
-        # self.rotation_vector = r
-        # # self.difference_vector = (d[0] - r[0], d[1] - r[1])
-
-        # print('direction:  ({0[0]:.2f}, {0[1]:.2f})'.format(self.direction_vector))
-        # print('rotation:   ({0[0]:.2f}, {0[1]:.2f})'.format(self.rotation_vector))
-        # print('difference: ({0[0]:.2f}, {0[1]:.2f})\n'.format(self.difference_vector))
-
-    def __to_polar(self, x, y):
-        # since atan2 knows both y and x, it yields the angle in the proper quadrant
-        return sqrt(x**2 + y**2), degrees(atan2(y, x))
-
-
 class JoyNode(Node):
     def __init__(
         self,
@@ -287,6 +245,7 @@ class JoyNode(Node):
     def run(self):
         msg = Joy()
         msg.axes = [0.0, 0.0]
+        msg.buttons = [0]
         with JoystickController("/dev/input/js0") as controller:
             while rclpy.ok():
                 controller.Update()
@@ -295,11 +254,16 @@ class JoyNode(Node):
                 if controller.hasUpdate():
                     hor = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_HORIZONTAL)
                     ver = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_VERTICAL)
+
+                    button_change_mode = controller.getButtonState(JSEvent.BUTTON_CIRCLE) # it says circle but it actually is dpad up
+                    self.get_logger().info(str(button_change_mode))
+
                     hor /= 2**15
                     ver /= 2**15
 
                     msg.axes[JSEvent.AXIS_LEFT_STICK_HORIZONTAL] = hor
                     msg.axes[JSEvent.AXIS_LEFT_STICK_VERTICAL] = ver
+                    msg.buttons[0] = button_change_mode
 
                     self.pub.publish(msg)
 

src/dcaitirobot/dcaitirobot/twist.py

@@ -24,6 +24,8 @@ class TwistPublisher(Node):
     ):
         super().__init__("twist_publisher")
 
+        self.mode = "manual"
+
         self.linear_window = np.zeros(smooth_weights.shape[0])
         self.angular_window = np.zeros(smooth_weights.shape[0])
         self.linear_recent = 0.0
@@ -38,9 +40,9 @@ class TwistPublisher(Node):
         self.linear_scale = linear_scale
         self.angular_scale = angular_scale
 
-        self.publisher_ = self.create_publisher(
+        # self.publisher_ = self.create_publisher(
-            Twist, "/diff_cont/cmd_vel_unstamped", 1
+        #     Twist, "/diff_cont/cmd_vel_unstamped", 1
-        )
+        # )
 
         self.publisher_2 = self.create_publisher(Twist, "/cmd_vel_joystick", 1)
 
@@ -52,6 +54,11 @@ class TwistPublisher(Node):
         self.linear_recent = -y * SCALE_LINEAR_VELOCITY
         self.angular_recent = x * SCALE_ANGULAR_VELOCITY
 
+        if joy_message.buttons[0]:
+            self.mode = "manual" if self.mode == "auto" else "auto"
+            self.get_logger().info(f"Switching to {self.mode!r} mode")
+
+  
     def update_smooth_window(self):
         self.linear_window = np.append(self.linear_window, self.linear_recent)[
             -self.window_size :
@@ -65,6 +72,10 @@ class TwistPublisher(Node):
         self.angular_out = np.average(self.angular_window, weights=self.weights)
 
     def timer_callback(self):
+        
+        if self.mode != "manual":
+            return
+
         msg = Twist()
 
         self.update_smooth_window()
@@ -72,16 +83,16 @@ class TwistPublisher(Node):
 
         msg.linear.x = self.linear_out
         msg.angular.z = self.angular_out
-        self.publisher_.publish(msg)
+        # self.publisher_.publish(msg)
         self.publisher_2.publish(msg)
-        self.get_logger().info(
+        # self.get_logger().info(
-            "Linear_window: {}, Linear_out: {}, Angular_window: {}, Angular_out: {}".format(
+        #     "Linear_window: {}, Linear_out: {}, Angular_window: {}, Angular_out: {}".format(
-                self.linear_window,
+        #         self.linear_window,
-                self.linear_out,
+        #         self.linear_out,
-                self.angular_window,
+        #         self.angular_window,
-                self.angular_out,
+        #         self.angular_out,
-            )
+        #     )
-        )
+        # )
 
 
 def main(args=None):

src/dcaitirobot/embedded/src/communication/UARTCom.cpp

@@ -171,7 +171,7 @@ void handleRequest (uint8_t* d)
 			break;
 		}
 		case DRIVE_ENCODER: {
-			UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM(), wheelLeft.getPosition(), wheelRight.getPosition());
+			UValue::sendBothEncoderValues(wheelRight.getGearedSpeedRPM(), -wheelLeft.getGearedSpeedRPM(), wheelRight.getPosition(), -wheelLeft.getPosition());
 			break;
 		}
 		case PID_PARAMETER: {

src/dcaitirobot/launch/launch_slam.py

@@ -10,7 +10,8 @@ from ament_index_python import get_package_share_directory
 
 def generate_launch_description():
     slam_params = os.path.join(
-        str(Path(__file__).absolute().parent.parent), # <-- Replace with your package name
+        str(Path(__file__).absolute().parent.parent.parent), # <-- Replace with your package name
+        'dcaiti_navigation',
         'config',
         'slam.yml'
         )
@@ -24,6 +25,22 @@ def generate_launch_description():
                     'slam_params_file': slam_params}.items()
     )
 
+    node_fix_lidar = Node(
+            package="dcaitirobot",
+            executable="fixlidar",
+            name="fixlidar",
+        )
+
+    tfs1 = Node(package = "tf2_ros", 
+                       executable = "static_transform_publisher",
+                       arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "velodyne"])
+
+    tfs2 = Node(package = "tf2_ros", 
+                       executable = "static_transform_publisher",
+                       arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "base_footprint"])
+
+
+
     return LaunchDescription([
-        slam_toolbox
+        slam_toolbox, node_fix_lidar, tfs1, tfs2
     ])

src/m-explore-ros2

@@ -1 +1 @@
-Subproject commit 81fdebe979ddf142065cc276989124437976978b
+Subproject commit e40e857b6af6e64f82514efdcd2cb9844ea77dc4