local changes merged master

src/dcaiti_control/config/dcaiti_config.yml

@@ -13,18 +13,23 @@ diff_cont:
 
     publish_rate: 30.0 # You can set this higher than the controller manager update rate, but it will be throttled
     base_frame_id: base_link
+    publish_limited_velocity: true
 
     left_wheel_names: ['left_wheel_joint']
     right_wheel_names: ['right_wheel_joint']
     wheel_separation: 0.215
     wheel_radius: 0.049338032
 
-#    linear:
-#      x:
-#        max_acceleration: 0.15
-#        has_acceleration_limits: true
 
     use_stamped_vel: false
 
-# joint_broad:
-#   ros__parameters:
+joint_limits:
+  ros__parameters:
+    left_wheel_joint:
+      has_position_limits: false # Continuous joint
+      has_velocity_limits: true
+      max_velocity: 10.0
+    right_wheel_joint:
+      has_position_limits: false # Continuous joint
+      has_velocity_limits: true
+      max_velocity: 10.0

src/dcaiti_control/config/empty.yaml

@@ -1 +0,0 @@
-# This is an empty file, so that git commits the folder correctly

src/dcaiti_control/description/lidar.xacro

@@ -34,19 +34,19 @@
  
         <sensor name="laser" type="ray">
             <pose> 0 0 0 0 0 0 </pose>
-            <visualize>true</visualize>
+            <visualize>false</visualize>
             <update_rate>10</update_rate>
             <ray>
                 <scan>
                     <horizontal>
-                        <samples>360</samples>
+                        <samples>360*5</samples>
                         <min_angle>-3.14</min_angle>
                         <max_angle>3.14</max_angle>
                     </horizontal>
                 </scan>
                 <range>
                     <min>0.3</min>
-                    <max>25</max>
+                    <max>100</max>
                 </range>
             </ray>
             <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">

src/dcaiti_control/description/robot.urdf.xacro

@@ -5,6 +5,24 @@
     <xacro:arg name="use_ros2_control" default="true"/>
     <xacro:arg name="use_sim" default="false"/>
 
+    <xacro:arg name="tube_thickness" default="0.024"/>
+    <xacro:arg name="tube_radius" default="false"/>
+
+    <xacro:arg name="camera_height" default="0.201"/>
+    <xacro:arg name="camera_offset_x" default="0.016"/>
+
+    <xacro:arg name="lidar_height" default="0.201"/>
+
+    <xacro:arg name="wheel_radius" default="0.05"/>
+    <xacro:arg name="wheel_width" default="0.02"/>
+
+    <xacro:arg name="base_height" default="0.1"/>
+    <xacro:arg name="base_width" default="0.2"/>
+    <xacro:arg name="base_length" default="0.2"/>
+
+
+
+
     <!-- Include the colours -->
     <xacro:include filename="colours.xacro" />
     <!-- Include the inertial macros -->

src/dcaiti_control/description/ros2_control.xacro

@@ -9,7 +9,7 @@
                 <param name="min">-0.5</param>
                 <param name="max">0.5</param>
             </command_interface>
-            <!-- <state_interface name="position" /> -->
+            <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>
         <joint name="right_wheel_joint">
@@ -18,7 +18,7 @@
                 <param name="max">0.5</param>
             </command_interface>
             <state_interface name="velocity" />
-            <!--  <state_interface name="position" /> -->
+            <state_interface name="position" />
         </joint>
     </ros2_control>
     <gazebo>

src/dcaiti_control/launch/rsp.launch.py

@@ -27,7 +27,6 @@ def generate_launch_description():
     node_robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
-        output='screen',
         parameters=[params]
     )
 

src/dcaiti_navigation/config/ekf.yaml

@@ -0,0 +1,233 @@
+### ekf config file ###
+ekf_filter_node:
+    ros__parameters:
+        
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of the inputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+        frequency: 30.0
+
+# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+# filter will generate new output. Defaults to 1 / frequency if not specified.
+        sensor_timeout: 0.1
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+        two_d_mode: true
+
+# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+# unspecified.
+        transform_time_offset: 0.0
+
+# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 
+# Defaults to 0.0 if unspecified.
+        transform_timeout: 0.0
+
+# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+# unhappy with any settings or data.
+        print_diagnostics: true
+
+# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+# effects on the performance of the node. Defaults to false if unspecified.
+        debug: false
+
+# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
+        debug_out_file: /home/focalfossa/dev_ws/src/basic_mobile_robot/debug_ekf.txt
+
+# Whether we'll allow old measurements to cause a re-publication of the updated state        
+        permit_corrected_publication: false
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+        publish_acceleration: false
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+        publish_tf: true
+        
+# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+# Here is how to use the following settings:
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+#         odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+# from landmark observations) then:
+#     3a. Set your "world_frame" to your map_frame value
+#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
+        map_frame: map                   # Defaults to "map" if unspecified
+        odom_frame: odom                 # Defaults to "odom" if unspecified
+        base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
+        world_frame: odom                # Defaults to the value of odom_frame if unspecified
+
+# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+# default values, and must be specified.
+        odom0: wheel/odometry
+
+# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+# if unspecified, effectively making this parameter required for each sensor.
+        odom0_config: [true, true, true,
+                       false, false, false,
+                       true,  true,  true,
+                       false, false, true,
+                       false, false, false]
+
+        #        [x_pos   , y_pos    , z_pos,
+        #         roll    , pitch    , yaw,
+        #         x_vel   , y_vel    , z_vel,
+        #         roll_vel, pitch_vel, yaw_vel,
+        #         x_accel , y_accel  , z_accel]
+
+# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
+# the size of the subscription queue so that more measurements are fused.
+        odom0_queue_size: 2
+
+# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
+# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
+# algorithm.
+        odom0_nodelay: false
+
+# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+# for twist measurements has no effect.
+        odom0_differential: false
+
+# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
+# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
+# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
+# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
+        odom0_relative: false
+
+# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
+# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
+# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
+# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
+# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
+# the thresholds.
+        odom0_pose_rejection_threshold: 5.0
+        odom0_twist_rejection_threshold: 1.0
+
+        imu0: imu/data
+        imu0_config: [false, false, false,
+                      true,  true,  true,
+                      false, false, false,
+                      true,  true,  true,
+                      true,  true,  true]
+
+        #        [x_pos   , y_pos    , z_pos,
+        #         roll    , pitch    , yaw,
+        #         x_vel   , y_vel    , z_vel,
+        #         roll_vel, pitch_vel, yaw_vel,
+        #         x_accel , y_accel  , z_accel]
+        
+        imu0_nodelay: false
+        imu0_differential: false
+        imu0_relative: true
+        imu0_queue_size: 5
+        imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
+        imu0_twist_rejection_threshold: 0.8                #
+        imu0_linear_acceleration_rejection_threshold: 0.8  #
+
+# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+        imu0_remove_gravitational_acceleration: true
+
+# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
+# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
+# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
+# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
+# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
+# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
+# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
+# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
+# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
+# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
+# inputs, the control term will be ignored.
+# Whether or not we use the control input during predicition. Defaults to false.
+        use_control: false
+# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+# false.
+        stamped_control: false
+# The last issued control command will be used in prediction for this period. Defaults to 0.2.
+        control_timeout: 0.2
+# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+        control_config: [true, false, false, false, false, true]
+# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+# Acceleration and deceleration limits are not always the same for robots.
+        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+# gains
+        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+# gains
+        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+# However, if users find that a given variable is slow to converge, one approach is to increase the
+# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+# unspecified.
+        process_noise_covariance: [0.05,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.05,   0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.06,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.03,   0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.03,   0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.06,   0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025,   0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01,   0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01,   0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.02,   0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.01,   0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.01,   0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]
+# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+# question. Users should take care not to use large values for variables that will not be measured directly. The values
+# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
+#if unspecified.
+        initial_estimate_covariance: [1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,    0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1e-9,    0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9,    0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1e-9,   0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1e-9,   0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]
+

src/dcaiti_navigation/config/nav2.yml

@@ -0,0 +1,11 @@
+planner_server:
+  ros__parameters:
+    planner_plugins: ['GridBased']
+    GridBased:
+      plugin: 'nav2_navfn_planner/NavfnPlanner'
+
+controller_server:
+  ros__parameters:
+    controller_plugins: ["FollowPath"]
+    FollowPath:
+       plugin: "dwb_core::DWBLocalPlanner"

src/dcaiti_navigation/launch/start_nav2.py

@@ -0,0 +1,144 @@
+# Author: Addison Sears-Collins
+# Date: September 2, 2021
+# Description: Launch a basic mobile robot using the ROS 2 Navigation Stack
+# https://automaticaddison.com
+
+import os
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, LaunchConfiguration, PythonExpression
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+
+  # Set the path to different files and folders.
+  pkg_share = get_package_share_directory('dcaiti_control')
+  world_file_name = 'basic_mobile_bot_world/smalltown.world'
+  world_path = os.path.join(pkg_share, 'worlds', world_file_name)
+  static_map_path = os.path.join(pkg_share, 'maps', 'smalltown_world.yaml')
+  nav2_params_path = os.path.join(pkg_share, 'params', 'nav2_params.yaml')
+  behavior_tree_xml_path = os.path.join(get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')
+  
+  # Launch configuration variables specific to simulation
+  autostart = LaunchConfiguration('autostart')
+  default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
+  map_yaml_file = LaunchConfiguration('map')
+  namespace = LaunchConfiguration('namespace')
+  params_file = LaunchConfiguration('params_file')
+  slam = LaunchConfiguration('slam')
+  use_namespace = LaunchConfiguration('use_namespace')
+  use_sim_time = LaunchConfiguration('use_sim_time')
+  
+  # Map fully qualified names to relative ones so the node's namespace can be prepended.
+  # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
+  # https://github.com/ros/geometry2/issues/32
+  # https://github.com/ros/robot_state_publisher/pull/30
+  # TODO(orduno) Substitute with `PushNodeRemapping`
+  #              https://github.com/ros2/launch_ros/issues/56
+  remappings = [('/tf', 'tf'),
+                ('/tf_static', 'tf_static')]
+  
+  # Declare the launch arguments  
+  declare_namespace_cmd = DeclareLaunchArgument(
+    name='namespace',
+    default_value='',
+    description='Top-level namespace')
+
+  declare_use_namespace_cmd = DeclareLaunchArgument(
+    name='use_namespace',
+    default_value='False',
+    description='Whether to apply a namespace to the navigation stack')
+        
+  declare_autostart_cmd = DeclareLaunchArgument(
+    name='autostart', 
+    default_value='true',
+    description='Automatically startup the nav2 stack')
+
+  declare_bt_xml_cmd = DeclareLaunchArgument(
+    name='default_bt_xml_filename',
+    default_value=behavior_tree_xml_path,
+    description='Full path to the behavior tree xml file to use')
+        
+  declare_map_yaml_cmd = DeclareLaunchArgument(
+    name='map',
+    default_value=static_map_path,
+    description='Full path to map file to load')
+    
+  declare_params_file_cmd = DeclareLaunchArgument(
+    name='params_file',
+    default_value=nav2_params_path,
+    description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+  declare_simulator_cmd = DeclareLaunchArgument(
+    name='headless',
+    default_value='False',
+    description='Whether to execute gzclient')
+
+  declare_slam_cmd = DeclareLaunchArgument(
+    name='slam',
+    default_value='False',
+    description='Whether to run SLAM')
+    
+  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
+    name='use_robot_state_pub',
+    default_value='True',
+    description='Whether to start the robot state publisher')
+
+  declare_use_rviz_cmd = DeclareLaunchArgument(
+    name='use_rviz',
+    default_value='True',
+    description='Whether to start RVIZ')
+    
+  declare_use_sim_time_cmd = DeclareLaunchArgument(
+    name='use_sim_time',
+    default_value='True',
+    description='Use simulation (Gazebo) clock if true')
+
+  declare_use_simulator_cmd = DeclareLaunchArgument(
+    name='use_simulator',
+    default_value='True',
+    description='Whether to start the simulator')
+
+  declare_world_cmd = DeclareLaunchArgument(
+    name='world',
+    default_value=world_path,
+    description='Full path to the world model file to load')
+   
+  # Specify the actions   
+
+  # Launch the ROS 2 Navigation Stack
+  start_ros2_navigation_cmd = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(get_package_share_directory('nav2'),'launch','bringup_launch.py'),
+    launch_arguments = {'namespace': namespace,
+                        'use_namespace': use_namespace,
+                        'slam': slam,
+                        'map': map_yaml_file,
+                        'use_sim_time': use_sim_time,
+                        'params_file': params_file,
+                        'default_bt_xml_filename': default_bt_xml_filename,
+                        'autostart': autostart}.items())
+
+  # Create the launch description and populate
+  ld = LaunchDescription()
+
+  # Declare the launch options
+  ld.add_action(declare_namespace_cmd)
+  ld.add_action(declare_use_namespace_cmd)
+  ld.add_action(declare_autostart_cmd)
+  ld.add_action(declare_bt_xml_cmd)
+  ld.add_action(declare_map_yaml_cmd)
+  ld.add_action(declare_params_file_cmd)
+  ld.add_action(declare_simulator_cmd)
+  ld.add_action(declare_slam_cmd)
+  ld.add_action(declare_use_robot_state_pub_cmd)  
+  ld.add_action(declare_use_rviz_cmd) 
+  ld.add_action(declare_use_sim_time_cmd)
+  ld.add_action(declare_use_simulator_cmd)
+  ld.add_action(declare_world_cmd)
+
+  # Add any actions
+  ld.add_action(start_ros2_navigation_cmd)
+
+  return ld

src/dcaiti_navigation/maps/my_map.yaml

@@ -0,0 +1,7 @@
+image: my_map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-18.7, -17.5, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/dcaiti_navigation/maps/smalltown_world.yaml

@@ -0,0 +1,6 @@
+image: smalltown_world.pgm
+resolution: 0.050000
+origin: [-51.224998, -51.224998, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

src/dcaiti_navigation/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>dcaiti_navigation</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="you@example.com">korjakow</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/dcaiti_navigation/params/nav2_params.yaml

@@ -0,0 +1,403 @@
+amcl:
+  ros__parameters:
+    use_sim_time: True
+    alpha1: 0.002
+    alpha2: 0.002
+    alpha3: 0.002
+    alpha4: 0.002
+    alpha5: 0.002
+    base_frame_id: "base_link"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    initial_pose: [0.0, 0.0, 0.0, 0.0]
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "differential"
+    save_pose_rate: 0.5
+    set_initial_pose: False
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: True
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /diff_cont/odom #/wheel/odometry
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    enable_groot_monitoring: True
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 2.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    #precise_goal_checker:
+    #  plugin: "nav2_controller::SimpleGoalChecker"
+    #  xy_goal_tolerance: 0.25
+    #  yaw_goal_tolerance: 0.25
+    #  stateful: True
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0 
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 10
+      height: 10
+      resolution: 0.05
+      robot_radius: 0.75
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 10.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 9.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.75
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 10.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 9.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.95
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: True
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: True
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    yaml_filename: "turtlebot3_world.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: True
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 2.0
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+    
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 2
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+    
+slam_toolbox:
+  ros__parameters:
+
+    # Plugin params
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ROS Parameters
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_link
+    scan_topic: /scan
+    mode: mapping #localization
+
+    # 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
+    # will use pose
+    #map_file_name: test_steve
+    #map_start_pose: [0.0, 0.0, 0.0]
+    #map_start_at_dock: true
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 5.0
+    resolution: 0.05
+    max_laser_range: 20.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    enable_interactive_mode: true
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.5
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1  
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true 
+    loop_match_minimum_chain_size: 10           
+    loop_match_maximum_variance_coarse: 3.0  
+    loop_match_minimum_response_coarse: 0.35    
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation Parameters - Correlation Parameters
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1 
+
+    # Correlation Parameters - Loop Closure Parameters
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Scan Matcher Parameters
+    distance_variance_penalty: 0.5      
+    angle_variance_penalty: 1.0    
+
+    fine_search_angle_offset: 0.00349     
+    coarse_search_angle_offset: 0.349   
+    coarse_angle_resolution: 0.0349        
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true

src/dcaiti_navigation/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/dcaiti_navigation
+[install]
+install_scripts=$base/lib/dcaiti_navigation

src/dcaiti_navigation/setup.py

@@ -0,0 +1,28 @@
+from glob import glob
+import os
+from setuptools import find_packages, setup
+
+package_name = 'dcaiti_navigation'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name), glob('launch/*.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='korjakow',
+    maintainer_email='you@example.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

src/dcaiti_navigation/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/dcaiti_navigation/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/dcaiti_navigation/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/dcaitirobot/dcaitirobot/scale_odom.py

@@ -0,0 +1,65 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+from nav_msgs.msg import Odometry
+
+import numpy as np
+
+
+SCALE_FACTOR = 20.0
+
+
+
+class TwistPublisher(Node):
+    def __init__(
+        self,
+        scale_factor=20.0
+    ):
+        super().__init__("twist_publisher")
+
+        self.scale_factor = scale_factor
+
+        self.publisher_ = self.create_publisher(
+            Odometry, "/odom_scaled", 1
+        )
+
+        self.create_subscription(Odometry, "/odom", self.odom_callback, 1)
+
+    def odom_callback(self, odom_message:Odometry):
+        odom_message.twist.twist.linear.x *= self.scale_factor
+        odom_message.twist.twist.linear.y *= self.scale_factor
+        odom_message.twist.twist.linear.z *= self.scale_factor
+        odom_message.twist.twist.angular.x *= self.scale_factor
+        odom_message.twist.twist.angular.y *= self.scale_factor
+        odom_message.twist.twist.angular.z *= self.scale_factor
+        odom_message.pose.pose.position.x *= self.scale_factor
+        odom_message.pose.pose.position.y *= self.scale_factor
+        odom_message.pose.pose.position.z *= self.scale_factor
+        odom_message.pose.pose.orientation.x *= self.scale_factor
+        odom_message.pose.pose.orientation.y *= self.scale_factor
+        odom_message.pose.pose.orientation.z *= self.scale_factor
+
+        self.publisher_.publish(odom_message)
+
+
+
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_publisher = TwistPublisher(
+        SCALE_FACTOR
+    )
+
+    rclpy.spin(minimal_publisher)
+
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/dcaitirobot/embedded/include/communication/UValue.hpp

@@ -37,7 +37,7 @@ public:
 	 * @param value_left 	encoder value left
 	 * @param value_right 	encoder value right
 	 */
-	static void sendBothEncoderValues(float value_left, float value_right);
+	static void sendBothEncoderValues(float speed_right, float speed_left, float pos_right, float pos_left);
 };
 
 #endif

src/dcaitirobot/embedded/include/misc/Utils.hpp

@@ -13,7 +13,6 @@
  */
 
 #include "Arduino.h"
-#include "R2WD.h"
 
 class Utils {
 private:

src/dcaitirobot/embedded/platformio.ini

@@ -13,5 +13,3 @@ platform = atmelavr
 board = diecimilaatmega328
 framework = arduino
 monitor_speed = 115200
-lib_deps = 
-    arduino-libraries

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

@@ -3,6 +3,7 @@
 #include "communication/UValue.hpp"
 #include "misc/Utils.hpp"
 #include "constants.h"
+#include "MotorWheel.h"
 
 uint8_t UARTCom::data[MAX_PACKET_LENGTH];
 uint8_t currentParseIndex;
@@ -170,7 +171,7 @@ void handleRequest (uint8_t* d)
 			break;
 		}
 		case DRIVE_ENCODER: {
-			UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM());
+			UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM(), wheelLeft.getPosition(), wheelRight.getPosition());
 			break;
 		}
 		case PID_PARAMETER: {

src/dcaitirobot/embedded/src/communication/UCommands.cpp

@@ -1,5 +1,7 @@
 #include "communication/UCommands.hpp"
 #include "misc/Utils.hpp"
+#include "MotorWheel.h"
+
 extern MotorWheel wheelLeft;
 extern MotorWheel wheelRight;
 

src/dcaitirobot/embedded/src/communication/UValue.cpp

@@ -19,20 +19,24 @@ void UValue::sendEncoderValue (U_Component cp, float value)
 	UARTCom::send(VALUE, 4, cp, switched_b);
 }
 
-void UValue::sendBothEncoderValues (float value_right, float value_left)
+void UValue::sendBothEncoderValues (float speed_right, float speed_left, float pos_right, float pos_left)
 {
 	union {
-		float f[2];
-		uint8_t b[8];
+		float f[4];
+		uint8_t b[16];
 	} u;
-	u.f[0] = value_right;
-	u.f[1] = value_left;
+	u.f[0] = speed_right;
+	u.f[1] = speed_left;
+	u.f[2] = pos_right;
+	u.f[3] = pos_left;
 
-	uint8_t switched_byte_array[8];
+	uint8_t switched_byte_array[16];
 	for (int i = 0; i < 4; i++) {
 		switched_byte_array[i] = u.b[3 - i];
 		switched_byte_array[i+4] = u.b[7 - i];
+		switched_byte_array[i+8] = u.b[11 - i];
+		switched_byte_array[i+12] = u.b[15 - i];
 	}
 
-	UARTCom::send(VALUE, 8, ALL_MOTORS, switched_byte_array);
+	UARTCom::send(VALUE, 16, ALL_MOTORS, switched_byte_array);
 }

src/dcaitirobot/launch/launch_slam.py

@@ -15,14 +15,12 @@ def generate_launch_description():
         'slam.yml'
         )
     
-    print(slam_params)
-
     slam_toolbox = IncludeLaunchDescription(
                 PythonLaunchDescriptionSource([os.path.join(
                     get_package_share_directory('slam_toolbox'),'launch','online_async_launch.py'
                 )]), 
                 launch_arguments={
-                    'use_sim_time': 'false', 
+                    'use_sim_time': 'true', 
                     'slam_params_file': slam_params}.items()
     )
 

src/dcaitirobot/setup.py

@@ -13,7 +13,7 @@ setup(
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         (os.path.join('share', package_name), glob('launch/*.py')),
-        (os.path.join('share', package_name, 'config'), glob('config/*.yml')),
+        (os.path.join('share', 'config'), glob('config/*.yml')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,

src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h

@@ -34,11 +34,11 @@ public:
 
   void setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms);
   void sendEmptyMsg();
-  void readEncoderValues(double &val_1, double &val_2);
+  void readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos);
   void setMotorValues(float val_1, float val_2);
   void setPidValues(float k_p, float k_d, float k_i, float k_o);
   const std::vector<uint8_t> buildMsg(uint8_t frame_type, uint8_t frame_component, long time,  std::vector<uint8_t> payload);
-  void readSpeed(double &left_speed, double &right_speed);
+  void readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos);
 
   bool connected() const { return serial_conn_.isOpen(); }
 

src/diffdrive_arduino/src/arduino_comms.cpp

@@ -21,12 +21,12 @@ void ArduinoComms::sendEmptyMsg()
 {
 }
 
-void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed)
+void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos)
 {   
     std::vector<uint8_t> payload = {DRIVE_ENCODER};
     std::vector<uint8_t> msg = buildMsg(REQUEST, BOTH_MOTORS, 0, payload);
     sendMsg(msg);
-    readSpeed(left_speed, right_speed);
+    readSpeedandPosition(left_speed, right_speed, left_pos, right_pos);
 }
 
 void ArduinoComms::setMotorValues(float left_speed, float right_speed)
@@ -49,7 +49,7 @@ void ArduinoComms::setPidValues(float k_p, float k_d, float k_i, float k_o)
     ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r";
 }
 
-void ArduinoComms::readSpeed(double &left_speed, double &right_speed)
+void ArduinoComms::readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos)
 {
     std::vector<uint8_t> buffer;
     int read_bytes = 0;
@@ -91,13 +91,21 @@ void ArduinoComms::readSpeed(double &left_speed, double &right_speed)
     if (frame_type == VALUE && frame_component == BOTH_MOTORS)
     {
             std::vector<uint8_t> left_speed_bytes(payload.begin(), payload.begin() + 4);
-            std::vector<uint8_t> right_speed_bytes(payload.begin() + 4, payload.end());
+            std::vector<uint8_t> right_speed_bytes(payload.begin() + 4, payload.begin()+8);
+            std::vector<uint8_t> left_pos_bytes(payload.begin() + 8, payload.begin()+12);
+            std::vector<uint8_t> right_pos_bytes(payload.begin() + 12, payload.end());
             std::reverse(left_speed_bytes.begin(), left_speed_bytes.end());
             std::reverse(right_speed_bytes.begin(), right_speed_bytes.end());
+            std::reverse(left_pos_bytes.begin(), left_pos_bytes.end());
+            std::reverse(right_pos_bytes.begin(), right_pos_bytes.end());
             float left_speed_float = *reinterpret_cast<float*>(&left_speed_bytes[0]);
             float right_speed_float = *reinterpret_cast<float*>(&right_speed_bytes[0]);
+            float left_pos_float = *reinterpret_cast<float*>(&left_pos_bytes[0]);
+            float right_pos_float = *reinterpret_cast<float*>(&right_pos_bytes[0]);
             left_speed = left_speed_float;
             right_speed = right_speed_float;
+            left_pos = left_pos_float;
+            right_pos = right_pos_float;
     }
 }
 

src/diffdrive_arduino/src/diffdrive_arduino.cpp

@@ -7,6 +7,11 @@
 #include "rclcpp/rclcpp.hpp"
 #include <math.h>
 
+#define RADSTORPM(a)(a*60.0/(2.0*M_PI))
+#define RPMSTORADS(a)(a*2.0*M_PI/60.0)
+#define REVSTORAD(a)(a*2.0*M_PI)
+#define RADSTOREV(a)(a/(2.0*M_PI))
+
 namespace diffdrive_arduino
 {
 
@@ -106,18 +111,15 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp
     return return_type::ERROR;
   }
 
-  arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel);
-  // write l_wheel_.cmd to string and print via RCLCPP_INFO
-  std::string msg = "Left Wheel Read: " + std::to_string(l_wheel_.vel);
-  RCLCPP_INFO(logger_, msg.c_str());
+  arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel, l_wheel_.pos, r_wheel_.pos);
   // convert rpm to rad/s
-  l_wheel_.vel = (l_wheel_.vel / 60) * 2 * M_PI * 0.049 ;
-  r_wheel_.vel = (r_wheel_.vel / 60) * 2 * M_PI * 0.049 ;
+  l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
+  r_wheel_.vel = RPMSTORADS(r_wheel_.vel);
 
     
-  // Force the wheel position
-  l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
-  r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
+  // convert rev to rad
+  l_wheel_.pos = REVSTORAD(l_wheel_.pos);
+  r_wheel_.pos = REVSTORAD(r_wheel_.pos);
   return return_type::OK;  
 }
 
@@ -128,9 +130,7 @@ return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcp
     return return_type::ERROR;
   }
   // convert rad/s to rpm and write to motors
-  arduino_.setMotorValues(l_wheel_.cmd *  30 / M_PI , r_wheel_.cmd * 30 / M_PI );
-  std::string msg = "Left Wheel Command: " + std::to_string(l_wheel_.cmd* 30 / M_PI);
-  RCLCPP_INFO(logger_, msg.c_str());
+  arduino_.setMotorValues(RADSTORPM(l_wheel_.cmd), RADSTORPM(r_wheel_.cmd));
 
   return return_type::OK;  
 }