Merged AutoDRIVE-F1TENTH

ROS Workspace/autodrive/config/common_costmap_params.yaml

@@ -1,18 +1,20 @@
-footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ]
-transform_tolerance: 0.2
+footprint: [ [-0.25,-0.135], [0.25,-0.135], [0.25,0.135], [-0.25,0.135] ]
+# car length = 0.50   car width = 0.27
+# wheel base = 0.33   track width = 0.23
+# front offset = 0.09 rear offset = 0.08
+transform_tolerance: 1
 obstacle_layer:
   enabled: true
   obstacle_range: 3.0
   raytrace_range: 3.5
-  inflation_radius: 0.2
   track_unknown_space: false
   combination_method: 1
   observation_sources: lidar
-  lidar: {sensor_frame: lidar, data_type: LaserScan, topic: scan, marking: true, clearing: true}
+  lidar: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}
 inflation_layer:
   enabled: true
   cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10)
-  inflation_radius: 0.025 # max. distance from an obstacle at which costs are incurred for planning paths
+  inflation_radius: 0.15 # m (max. distance from an obstacle at which costs are incurred for planning paths)
 static_layer:
   enabled: true
   map_topic: "/map"

ROS Workspace/autodrive/config/global_costmap_params.yaml

@@ -1,7 +1,7 @@
 global_costmap:
   global_frame: map
-  robot_base_frame: nigel
-  update_frequency: 2.0
+  robot_base_frame: base_link
+  update_frequency: 5.0
   publish_frequency: 5.0
   transform_tolerance: 30
   width: 80

ROS Workspace/autodrive/config/local_costmap_params.yaml

@@ -1,11 +1,11 @@
 local_costmap:
   global_frame: odom
-  robot_base_frame: nigel
-  update_frequency: 2.0
+  robot_base_frame: base_link
+  update_frequency: 5.0
   publish_frequency: 5.0
   rolling_window: true
-  width: 1.5
-  height: 1.5
+  width: 3.0 # m
+  height: 3.0 # m
   resolution: 0.05
   transform_tolerance: 10
   plugins:

ROS Workspace/autodrive/config/planner_params.yaml

@@ -17,22 +17,25 @@ TebLocalPlannerROS:
   feasibility_check_no_poses: 2
   allow_init_backward_motion: false
   # Robot
-  max_vel_x: 0.2 # [sim max: 0.26 m/s]
-  max_vel_x_backwards: 0.2 # [sim max: 0.23 m/s]
-  max_vel_theta: 0.5236 # [sim max: 0.67 rad/s] angular velocity is also bounded by min_turning_radius in case of a carlike robot (omega = v/r)
+  # car length = 0.50   car width = 0.27
+  # wheel base = 0.33   track width = 0.23
+  # front offset = 0.09 rear offset = 0.08
+  max_vel_x: 0.4 # m/s
+  max_vel_x_backwards: 0.2 # m/s
+  max_vel_theta: 0.2 # rad/s [angular velocity is also bounded by min_turning_radius in case of a carlike robot (omega = v/r)]
   acc_lim_x: 0.15
   acc_lim_theta: 0.3927
   # Carlike robot parameters
-  min_turning_radius: 0.24515 # min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
-  wheelbase: 0.14154 # wheelbase of the carlike robot
+  min_turning_radius: 0.74 # min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
+  wheelbase: 0.33 # wheelbase of the carlike robot
   cmd_angle_instead_rotvel: True # return steering angle instead of the rotvel (angular.z of twist message)
   footprint_model:
     type: "line"
-    line_start: [-0.07077, 0.0] # for type "line"
-    line_end: [0.07077, 0.0] # for type "line"
+    line_start: [-0.18, 0.0] # for type "line"
+    line_end: [0.17, 0.0] # for type "line"
   # GoalTolerance
-  xy_goal_tolerance: 0.1
-  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.1 # m
+  yaw_goal_tolerance: 0.4 # rad
   free_goal_vel: False
   # Obstacles
   min_obstacle_dist: 0.2 # this value must also include our robot's expansion, since footprint_model is set to "line".
@@ -53,14 +56,14 @@ TebLocalPlannerROS:
   weight_acc_lim_x: 1
   weight_acc_lim_theta: 1
   weight_kinematics_nh: 1000
-  weight_kinematics_forward_drive: 100
+  weight_kinematics_forward_drive: 1000
   weight_kinematics_turning_radius: 1
   weight_optimaltime: 1
   weight_obstacle: 50
   weight_dynamic_obstacle: 10
   # Homotopy Class Planner
-  enable_homotopy_class_planning: True
-  enable_multithreading: True
+  enable_homotopy_class_planning: False
+  enable_multithreading: False
   simple_exploration: False
   max_number_classes: 4
   selection_cost_hysteresis: 1.0

ROS Workspace/autodrive/launch/testbed_amcl.launch

@@ -27,7 +27,7 @@
   <node pkg="autodrive" type="lidar_odometer" name="lidar_odometer">
     <param name="laser_scan_topic" value="/scan"/> # topic where the lidar scans are being published
     <param name="odom_topic" value="/odom"/>       # topic where to publish the odometry estimations
-    <param name="publish_tf" value="true"/>        # wheter or not to publish the tf::transform (base_frame->odom_frame)
+    <param name="publish_tf" value="true"/>        # whether or not to publish the tf::transform (base_frame->odom_frame)
     <param name="base_frame_id" value="/nigel"/>   # frame_id (tf) of the mobile base
     <param name="odom_frame_id" value="/odom"/>    # frame_id (tf) to publish the odometry estimations
     <param name="init_pose_from_topic" value=""/>  # leave empty to start at point (0,0)

ROS Workspace/autodrive/launch/testbed_amcl_remote.launch

@@ -18,7 +18,7 @@
   <node pkg="autodrive" type="lidar_odometer" name="lidar_odometer">
     <param name="laser_scan_topic" value="/scan"/> # topic where the lidar scans are being published
     <param name="odom_topic" value="/odom"/>       # topic where to publish the odometry estimations
-    <param name="publish_tf" value="true"/>        # wheter or not to publish the tf::transform (base_frame->odom_frame)
+    <param name="publish_tf" value="true"/>        # whether or not to publish the tf::transform (base_frame->odom_frame)
     <param name="base_frame_id" value="/nigel"/>   # frame_id (tf) of the mobile base
     <param name="odom_frame_id" value="/odom"/>    # frame_id (tf) to publish the odometry estimations
     <param name="init_pose_from_topic" value=""/>  # leave empty to start at point (0,0)

ROS Workspace/autodrive/launch/testbed_lidar_odometry.launch

@@ -21,7 +21,7 @@
   <node pkg="autodrive" type="lidar_odometer" name="lidar_odometer">
     <param name="laser_scan_topic" value="/scan"/> # topic where the lidar scans are being published
     <param name="odom_topic" value="/odom"/>       # topic where to publish the odometry estimations
-    <param name="publish_tf" value="true"/>        # wheter or not to publish the tf::transform (base_frame->odom_frame)
+    <param name="publish_tf" value="true"/>        # whether or not to publish the tf::transform (base_frame->odom_frame)
     <param name="base_frame_id" value="/nigel"/>   # frame_id (tf) of the mobile base
     <param name="odom_frame_id" value="/odom"/>    # frame_id (tf) to publish the odometry estimations
     <param name="init_pose_from_topic" value=""/>  # leave empty to start at point (0,0)

ROS Workspace/autodrive/launch/testbed_lidar_odometry_remote.launch

@@ -12,7 +12,7 @@
   <node pkg="autodrive" type="lidar_odometer" name="lidar_odometer">
     <param name="laser_scan_topic" value="/scan"/> # topic where the lidar scans are being published
     <param name="odom_topic" value="/odom"/>       # topic where to publish the odometry estimations
-    <param name="publish_tf" value="true"/>        # wheter or not to publish the tf::transform (base_frame->odom_frame)
+    <param name="publish_tf" value="true"/>        # whether or not to publish the tf::transform (base_frame->odom_frame)
     <param name="base_frame_id" value="/nigel"/>   # frame_id (tf) of the mobile base
     <param name="odom_frame_id" value="/odom"/>    # frame_id (tf) to publish the odometry estimations
     <param name="init_pose_from_topic" value=""/>  # leave empty to start at point (0,0)

ROS Workspace/autodrive/launch/testbed_navigation.launch

@@ -26,7 +26,7 @@
   <node pkg="autodrive" type="lidar_odometer" name="lidar_odometer">
     <param name="laser_scan_topic" value="/scan"/> # topic where the lidar scans are being published
     <param name="odom_topic" value="/odom"/>       # topic where to publish the odometry estimations
-    <param name="publish_tf" value="true"/>        # wheter or not to publish the tf::transform (base_frame->odom_frame)
+    <param name="publish_tf" value="true"/>        # whether or not to publish the tf::transform (base_frame->odom_frame)
     <param name="base_frame_id" value="/nigel"/>   # frame_id (tf) of the mobile base
     <param name="odom_frame_id" value="/odom"/>    # frame_id (tf) to publish the odometry estimations
     <param name="init_pose_from_topic" value=""/>  # leave empty to start at point (0,0)

ROS Workspace/autodrive/launch/testbed_navigation_remote.launch

@@ -17,7 +17,7 @@
   <node pkg="autodrive" type="lidar_odometer" name="lidar_odometer">
     <param name="laser_scan_topic" value="/scan"/> # topic where the lidar scans are being published
     <param name="odom_topic" value="/odom"/>       # topic where to publish the odometry estimations
-    <param name="publish_tf" value="true"/>        # wheter or not to publish the tf::transform (base_frame->odom_frame)
+    <param name="publish_tf" value="true"/>        # whether or not to publish the tf::transform (base_frame->odom_frame)
     <param name="base_frame_id" value="/nigel"/>   # frame_id (tf) of the mobile base
     <param name="odom_frame_id" value="/odom"/>    # frame_id (tf) to publish the odometry estimations
     <param name="init_pose_from_topic" value=""/>  # leave empty to start at point (0,0)

ROS Workspace/autodrive/scripts/nav_ctrl.py

@@ -2,14 +2,14 @@
 
 import rospy
 from geometry_msgs.msg import Twist
-from std_msgs.msg import Float32
+from ackermann_msgs.msg import AckermannDriveStamped
 
 ################################################################################
 
 DRIVE_LIMIT = 1
 STEER_LIMIT = 1
-THROTTLE_CONST = 1/0.26
-STEERING_CONST = -1/0.5236
+THROTTLE_CONST = 4
+STEERING_CONST = 1
 
 def constrain(input, low, high):
     if input < low:
@@ -30,15 +30,19 @@ def boundDrive(drive_cmd):
 
 ################################################################################
 
-steer_pub = rospy.Publisher('steer_cmd', Float32, queue_size=10)
-drive_pub = rospy.Publisher('drive_cmd', Float32, queue_size=10)
+control_pub = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/input/teleop', AckermannDriveStamped, queue_size=10)
+control_msg = AckermannDriveStamped()
 
 def navCtrlCallback(data):
     global throttle, steering
-    throttle = boundDrive(0.2 + (THROTTLE_CONST * data.linear.x))
+    throttle = boundDrive(THROTTLE_CONST * data.linear.x)
     steering = boundSteer(STEERING_CONST * data.angular.z)
-    drive_pub.publish(Float32(throttle))
-    steer_pub.publish(Float32(steering))
+
+    control_msg.header.stamp = rospy.Time.now()
+    control_msg.header.frame_id = 'base_link'
+    control_msg.drive.speed = throttle
+    control_msg.drive.steering_angle = steering
+    control_pub.publish(control_msg)
 
 ################################################################################
 

ROS Workspace/autodrive/scripts/teleop.py

@@ -1,7 +1,37 @@
 #!/usr/bin/env python
 
+'''
+BSD 2-Clause License
+
+Copyright (c) 2022, Tinker Twins
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+'''
+
+################################################################################
+
 import rospy
-from std_msgs.msg import Float32
+from ackermann_msgs.msg import AckermannDriveStamped
 import sys, select, os
 if os.name == 'nt':
   import msvcrt
@@ -10,28 +40,25 @@
 
 ################################################################################
 
-DRIVE_LIMIT = 1
+DRIVE_LIMIT = 5
 STEER_LIMIT = 1
-DRIVE_STEP_SIZE = 0.2
-STEER_STEP_SIZE = 0.2
+DRIVE_STEP_SIZE = 1
+STEER_STEP_SIZE = 1
 
 info = """
 -------------------------------------
-AutoDRIVE - Nigel Teleoperation Panel
+AutoDRIVE-F1TENTH Teleoperation Panel
+Developers - Chinmay and Tanmay Samak
 -------------------------------------
-
              Q   W   E
              A   S   D
                  X
-
 W/S : Increase/decrease drive command
 D/A : Increase/decrease steer command
 Q   : Zero steer
 E   : Emergency brake
 X   : Force stop and reset
-
 Press CTRL+C to quit
-
 NOTE: Press keys within this terminal
 -------------------------------------
 """
@@ -75,9 +102,9 @@ def boundDrive(drive_cmd):
     if os.name != 'nt':
         settings = termios.tcgetattr(sys.stdin)
 
-    rospy.init_node('testbed_teleop')
-    steer_pub = rospy.Publisher('steer_cmd', Float32, queue_size=10)
-    drive_pub = rospy.Publisher('drive_cmd', Float32, queue_size=10)
+    rospy.init_node('key_teleop')
+    key_teleop_pub = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/input/teleop', AckermannDriveStamped, queue_size=10)
+    key_teleop_msg = AckermannDriveStamped()
 
     throttle = 0.0
     steering = 0.0
@@ -92,9 +119,9 @@ def boundDrive(drive_cmd):
             elif key == 's' :
                 throttle = boundDrive(throttle - DRIVE_STEP_SIZE)
             elif key == 'a' :
-                steering = boundSteer(steering - STEER_STEP_SIZE)
-            elif key == 'd' :
                 steering = boundSteer(steering + STEER_STEP_SIZE)
+            elif key == 'd' :
+                steering = boundSteer(steering - STEER_STEP_SIZE)
             elif key == 'q' :
                 steering = 0.0
             elif key == 'e' :
@@ -106,17 +133,23 @@ def boundDrive(drive_cmd):
                 if (key == '\x03'): # CTRL+C
                     break
 
-            drive_pub.publish(throttle)
-            steer_pub.publish(steering)
+            key_teleop_msg.header.stamp = rospy.Time.now()
+            key_teleop_msg.header.frame_id = 'base_link'
+            key_teleop_msg.drive.speed = throttle
+            key_teleop_msg.drive.steering_angle = steering
+            key_teleop_pub.publish(key_teleop_msg)
 
     except:
         print(error)
 
     finally:
         throttle = 0.0
         steering = 0.0
-        drive_pub.publish(throttle)
-        steer_pub.publish(steering)
+        key_teleop_msg.header.stamp = rospy.Time.now()
+        key_teleop_msg.header.frame_id = 'base_link'
+        key_teleop_msg.drive.speed = throttle
+        key_teleop_msg.drive.steering_angle = steering
+        key_teleop_pub.publish(key_teleop_msg)
 
     if os.name != 'nt':
         termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

ROS Workspace/f1tenth/racecar/racecar/launch/camera_joy.launch

@@ -0,0 +1,25 @@
+<!-- -*- mode: XML -*- -->
+<launch>
+
+  <!-- F1TENTH Teleop Joystick Node -->
+  <arg name="racecar_version" default="racecar-v2" />
+  <arg name="run_camera" default="false"/>
+  <include file="$(find racecar)/launch/includes/$(arg racecar_version)-teleop.launch.xml">
+    <arg name="racecar_version" value="$(arg racecar_version)" />
+    <arg name="run_camera" value="$(arg run_camera)" />
+  </include>
+
+  <!-- F1TENTH Camera Node -->
+  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+    <param name="video_device" value="/dev/video0" />
+    <param name="image_width" value="640" />
+    <param name="image_height" value="480" />
+    <param name="pixel_format" value="yuyv" />
+    <param name="camera_frame_id" value="usb_cam" />
+    <param name="io_method" value="mmap"/>
+  </node>
+
+  <!-- RViz Node -->
+  <node name="rviz" pkg="rviz" type="rviz" respawn="false" output="screen" args="-d $(find racecar)/rviz/usb_webcam.rviz"/>
+
+</launch>

ROS Workspace/f1tenth/racecar/racecar/launch/camera_key.launch

@@ -0,0 +1,20 @@
+<!-- -*- mode: XML -*- -->
+<launch>
+
+  <!-- AutoDRIVE-F1TENTH Keyboard Teleop -->
+  <include file="$(find racecar)/launch/teleop_autodrive_key.launch"/>
+
+  <!-- F1TENTH Camera Node -->
+  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+    <param name="video_device" value="/dev/video0" />
+    <param name="image_width" value="640" />
+    <param name="image_height" value="480" />
+    <param name="pixel_format" value="yuyv" />
+    <param name="camera_frame_id" value="usb_cam" />
+    <param name="io_method" value="mmap"/>
+  </node>
+
+  <!-- RViz Node -->
+  <node name="rviz" pkg="rviz" type="rviz" respawn="false" output="screen" args="-d $(find racecar)/rviz/usb_webcam.rviz"/>
+
+</launch>

ROS Workspace/f1tenth/racecar/racecar/launch/data_recording_key.launch

@@ -5,8 +5,8 @@
   <arg name="run_camera" default="false"/>
   <arg name="rosbag_dir" default="$(find racecar)/rosbag"/>
 
-  <!-- F1TENTH Teleop Keyboard Node -->
-  <include file="$(find racecar)/launch/teleop_key.launch"/>
+  <!-- AutoDRIVE-F1TENTH Keyboard Teleop -->
+  <include file="$(find racecar)/launch/teleop_autodrive_key.launch"/>
 
   <!-- F1TENTH Camera Node -->
   <node name="usb_cam" pkg="usb_cam" type="usb_cam_node">

ROS Workspace/f1tenth/racecar/racecar/launch/hector_slam_key.launch

@@ -4,8 +4,8 @@
   <!-- Global Parameter -->
   <param name="/use_sim_time" value="false"/>
 
-  <!-- F1TENTH Teleop Keyboard Node -->
-  <include file="$(find racecar)/launch/teleop_key.launch"/>
+  <!-- AutoDRIVE-F1TENTH Keyboard Teleop -->
+  <include file="$(find racecar)/launch/teleop_autodrive_key.launch"/>
 
   <!-- F1TENTH LIDAR Node -->
   <node name="hokuyo_lidar" pkg="urg_node" type="urg_node">