merging 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:
+joint_limits:
-#   ros__parameters:
+  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>12</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/launch/launch_sim.py

@@ -25,6 +25,7 @@ def generate_launch_description():
     package_name='dcaiti_control' #<--- CHANGE ME
 
     use_ros2_control = LaunchConfiguration('use_ros2_control')
+    world_path='~/.gazebo/models'
 
     rsp = IncludeLaunchDescription(
                 PythonLaunchDescriptionSource([os.path.join(
@@ -36,7 +37,7 @@ def generate_launch_description():
     gazebo = IncludeLaunchDescription(
                 PythonLaunchDescriptionSource([os.path.join(
                     get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
-                    launch_arguments={'params_file': gazebo_params_path}.items()
+                    launch_arguments={'params_file': gazebo_params_path, 'world': world_path }.items()
              )
 
     # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.

src/dcaiti_control/launch/rsp.launch.py

@@ -28,7 +28,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/.gitignore

@@ -3,7 +3,6 @@ logs/
 install/
 build/
 bin/
-lib/
 log/
 msg_gen/
 srv_gen/

src/dcaitirobot/config/slam.yml

@@ -0,0 +1,6 @@
+slam_toolbox:
+  ros__parameters:
+    base_frame: base_link
+    map_update_interval: 1.0
+    scan_topic: /scan_no_nan
+    odom_topic: /odom

src/dcaitirobot/dcaitirobot/handle_nan_scan.py

@@ -0,0 +1,42 @@
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import LaserScan
+import geometry_msgs.msg as gm
+
+
+class TF2PublisherNode(Node):
+
+    def __init__(self):
+        super().__init__('nanhandler')
+        
+        self.create_subscription(LaserScan, "/scan", self.callback, 1)
+        self.pub = self.create_publisher(LaserScan, "/scan_no_nan", 1)
+    def callback(self, msg):
+        print(msg.ranges[0])
+        
+        msg.ranges[0] = float("inf")
+        msg.intensities[0] = 0.0
+
+        msg.ranges.append(float("inf"))
+        msg.intensities.append(0.0)
+        # ranges = np.array(msg.ranges)
+        # mask_inf_dist = np.isinf(ranges)
+        # print(np.array(msg.intensities)[mask_inf_dist])
+        
+        self.pub.publish(msg)
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    tf2_publisher = TF2PublisherNode()
+    rclpy.spin(tf2_publisher)
+
+    tf2_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

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 = 1.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/dcaitirobot/tf2pleasework.py

@@ -12,16 +12,14 @@ class TF2PublisherNode(Node):
         
         self.timer = self.create_timer(0.1, self.publish_identity_transform)
 
-        self.x = 0.0
+    def send_parent_child_identity_tf(self, parent, child):
-
-    def publish_identity_transform(self):
         transform_stamped = gm.TransformStamped()
         transform_stamped.header.stamp = self.get_clock().now().to_msg()
-        transform_stamped.header.frame_id = 'odom'
+        transform_stamped.header.frame_id = parent
-        transform_stamped.child_frame_id = 'base_link'
+        transform_stamped.child_frame_id = child
 
         # Identity transform (no translation, no rotation)
-        transform_stamped.transform.translation.x = self.x
+        transform_stamped.transform.translation.x = 0.0
         transform_stamped.transform.translation.y = 0.0
         transform_stamped.transform.translation.z = 0.0
         transform_stamped.transform.rotation.x = 0.0
@@ -29,28 +27,16 @@ class TF2PublisherNode(Node):
         transform_stamped.transform.rotation.z = 0.0
         transform_stamped.transform.rotation.w = 1.0
 
-
-        self.x += 0.01
-
-        self.tf_broadcaster.sendTransform(transform_stamped)
-
-        transform_stamped = gm.TransformStamped()
-        transform_stamped.header.stamp = self.get_clock().now().to_msg()
-        transform_stamped.header.frame_id = 'base_link'
-        transform_stamped.child_frame_id = 'velodyne'
-
-        # Identity transform (no translation, no rotation)
-        transform_stamped.transform.translation.x = 0.0
-        transform_stamped.transform.translation.y = self.x
-        transform_stamped.transform.translation.z = 0.0
-        transform_stamped.transform.rotation.x = 0.0
-        transform_stamped.transform.rotation.y = 0.0
-        transform_stamped.transform.rotation.z = 0.0
-        transform_stamped.transform.rotation.w = 1.0
-
         self.tf_broadcaster.sendTransform(transform_stamped)
 
 
+    def publish_identity_transform(self):
+        # self.send_parent_child_identity_tf("map", "odom")
+        # self.send_parent_child_identity_tf("odom", "base_link")
+       # self.send_parent_child_identity_tf("base_link", "base_scan")
+       # self.send_parent_child_identity_tf("base_scan", "velodyne")
+        self.send_parent_child_identity_tf("base_link", "velodyne")
+        self.send_parent_child_identity_tf("base_link", "base_footprint")
     
 
         self.get_logger().info('Published identity transform')

src/dcaitirobot/dcaitirobot/twist.py

@@ -7,8 +7,8 @@ from sensor_msgs.msg import Joy
 
 import numpy as np
 
-SCALE_LINEAR_VELOCITY = 0.1
+SCALE_LINEAR_VELOCITY = 0.2
-SCALE_ANGULAR_VELOCITY = 0.1
+SCALE_ANGULAR_VELOCITY = 0.7
 PUBLISH_FREQUENCY_HZ = 30
 
 SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])

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/lib/MotorWheel/MotorWheel.cpp

@@ -0,0 +1,385 @@
+#include<MotorWheel.h>
+
+Motor::Motor(unsigned char _pinPWM,unsigned char _pinDir,
+				unsigned char _pinIRQ,unsigned char _pinIRQB,
+				struct ISRVars* _isr)
+		:PID(&speedRPMInput,&speedRPMOutput,&speedRPMDesired,KC,TAUI,TAUD),
+		 pinPWM(_pinPWM),pinDir(_pinDir),isr(_isr) {
+	debug();
+
+	isr->pinIRQ=_pinIRQ;
+	isr->pinIRQB=_pinIRQB;
+
+/*for maple*/	
+#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
+	pinMode(pinPWM,PWM);
+	pinMode(pinDir,OUTPUT);
+	pinMode(isr->pinIRQ,INPUT_FLOATING);
+
+/*for arduino*/	
+#else
+	pinMode(pinPWM,OUTPUT);
+	pinMode(pinDir,OUTPUT);
+	pinMode(isr->pinIRQ,INPUT);
+	
+#endif
+	if(isr->pinIRQB!=PIN_UNDEFINED) {
+		pinMode(isr->pinIRQB,INPUT);
+	}
+
+	PIDDisable();
+}
+
+void Motor::setupInterrupt() {
+/*for maple*/
+#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
+	attachInterrupt(isr->pinIRQ,isr->ISRfunc,TRIGGER);	// RISING --> CHANGE 201207
+
+/*for arduino*/
+#else
+	if(isr->pinIRQ==2 || isr->pinIRQ==3) attachInterrupt(isr->pinIRQ-2,isr->ISRfunc,TRIGGER);
+	else {
+		PCattachInterrupt(isr->pinIRQ,isr->ISRfunc,TRIGGER);	// RISING --> CHANGE 201207
+	}
+//#ifndef _NAMIKI_MOTOR
+//	// Namiki motor can use 2 times only, because of the distance of the two optical sensors
+//	// 4 times of pulses, 201208
+//	if(isr->pinIRQB==2 || isr->pinIRQB==3) attachInterrupt(isr->pinIRQB-2,isr->ISRfunc,CHANGE);
+//	else {
+//		PCattachInterrupt(isr->pinIRQB,isr->ISRfunc,CHANGE);	// RISING --> CHANGE 201207
+//	}	
+//#endif
+#endif
+}
+
+unsigned char Motor::getPinPWM() const {
+	debug();
+	return pinPWM;
+}
+unsigned char Motor::getPinDir() const {
+	debug();
+	return pinDir;
+}
+unsigned char Motor::getPinIRQ() const {
+	debug();
+	return isr->pinIRQ;
+}
+unsigned char Motor::getPinIRQB() const {
+	debug();
+	return isr->pinIRQB;
+}
+
+unsigned int Motor::runPWM(unsigned int PWM,bool dir,bool saveDir) {
+	debug();
+	speedPWM=PWM;
+	if(saveDir) desiredDirection=dir;
+	analogWrite(pinPWM,PWM);
+	digitalWrite(pinDir,dir);
+	return PWM;
+}
+unsigned int Motor::advancePWM(unsigned int PWM) {
+	debug();
+	return runPWM(PWM,DIR_ADVANCE);
+}
+unsigned int Motor::backoffPWM(unsigned int PWM) {
+	debug();
+	return runPWM(PWM,DIR_BACKOFF);
+}
+unsigned int Motor::getPWM() const {
+	debug();
+	return speedPWM;
+}
+bool Motor::setDesiredDir(bool dir) {
+	debug();
+	//runPWM(getPWM(),dir);	// error
+	desiredDirection=dir;
+	return getDesiredDir();
+}
+bool Motor::getDesiredDir() const {
+	debug();
+	return desiredDirection;
+}
+bool Motor::reverseDesiredDir() {
+	debug();
+	runPWM(getPWM(),!getDesiredDir());
+	return getDesiredDir();
+}
+
+bool Motor::getCurrDir() const {
+	return isr->currDirection;
+}
+bool Motor::setCurrDir() {
+	if(getPinIRQB()!=PIN_UNDEFINED)
+		return isr->currDirection=digitalRead(isr->pinIRQB);
+	return false;
+}
+
+/*
+int Motor::getAccRPMM() const {
+	debug();
+	return SPEEDPPS2SPEEDRPM(isr->accPPSS);
+}
+ */
+/*
+unsigned int Motor::getSpeedRPM() const {
+	debug();
+	return SPEEDPPS2SPEEDRPM(isr->speedPPS);
+}
+ */
+unsigned int Motor::setSpeedRPM(int speedRPM,bool dir) {
+	debug();
+	PIDSetSpeedRPMDesired(speedRPM);
+	setDesiredDir(dir);
+	return abs(getSpeedRPM());
+}
+// direction sensitive 201208	
+int Motor::getSpeedRPM() const {
+	debug();
+	if(getCurrDir()==DIR_ADVANCE)
+		return SPEEDPPS2SPEEDRPM(isr->speedPPS);
+	return -SPEEDPPS2SPEEDRPM(isr->speedPPS);
+}
+int Motor::setSpeedRPM(int speedRPM) {
+	debug();
+	if(speedRPM>=0) return setSpeedRPM(speedRPM,DIR_ADVANCE);
+	else return setSpeedRPM(abs(speedRPM),DIR_BACKOFF);
+}
+
+
+bool Motor::PIDGetStatus() const {
+	debug();
+	return pidCtrl;
+}
+bool Motor::PIDEnable(float kc,float taui,float taud,unsigned int sampleTime) {
+	debug();
+	setupInterrupt();
+	PIDSetup(kc,taui,taud,sampleTime);
+	return pidCtrl=true;
+}
+bool Motor::PIDDisable() {
+	debug();
+
+	return pidCtrl=false;
+}
+bool Motor::PIDReset() {
+	debug();
+	if(PIDGetStatus()==false) return false;
+	PID::Reset();
+	return true;
+}
+
+bool Motor::PIDSetup(float kc,float taui,float taud,unsigned int sampleTime) {
+	debug();
+	PID::SetTunings(kc,taui,taud);
+	PID::SetInputLimits(0,MAX_SPEEDRPM);
+	PID::SetOutputLimits(0,MAX_SPEEDRPM);
+	PID::SetSampleTime(sampleTime);
+	PID::SetMode(AUTO);
+	return true;
+}
+unsigned int Motor::PIDSetSpeedRPMDesired(unsigned int speedRPM) {
+	debug();
+	if(speedRPM>MAX_SPEEDRPM) speedRPMDesired=MAX_SPEEDRPM;
+	else speedRPMDesired=speedRPM;
+	return PIDGetSpeedRPMDesired();
+}
+unsigned int Motor::PIDGetSpeedRPMDesired() const {
+	debug();
+	return speedRPMDesired;
+}
+
+bool Motor::PIDRegulate(bool doRegulate) {
+	debug();
+	if(PIDGetStatus()==false) return false;
+	if(getPinIRQB()!=PIN_UNDEFINED && getDesiredDir()!=getCurrDir()) {
+		speedRPMInput=-SPEEDPPS2SPEEDRPM(isr->speedPPS);
+	} else {
+		speedRPMInput=SPEEDPPS2SPEEDRPM(isr->speedPPS);
+	}
+
+	PID::Compute();
+	if(doRegulate && PID::JustCalculated()) {
+		speed2DutyCycle+=speedRPMOutput;
+
+		if(speed2DutyCycle>=MAX_SPEEDRPM) speed2DutyCycle=MAX_SPEEDRPM;
+		else if(speed2DutyCycle<=-MAX_SPEEDRPM)  speed2DutyCycle=-MAX_SPEEDRPM;
+		if(speed2DutyCycle>=0) {
+			runPWM(map(speed2DutyCycle,0,MAX_SPEEDRPM,0,MAX_PWM),getDesiredDir(),false);
+		} else {
+			runPWM(map(abs(speed2DutyCycle),0,MAX_SPEEDRPM,0,MAX_PWM),!getDesiredDir(),false);
+		}
+		return true;
+	}
+	return false;
+}
+
+int Motor::getSpeedPPS() const {
+	return isr->speedPPS;
+}
+long Motor::getCurrPulse() const {
+	return isr->pulses;
+}
+long Motor::setCurrPulse(long _pulse) {
+	isr->pulses=_pulse;
+	return getCurrPulse();
+}
+long Motor::resetCurrPulse() {
+	return setCurrPulse(0);
+}
+
+void Motor::delayMS(unsigned int ms,bool debug) {
+	for(unsigned long endTime=millis()+ms;millis()<endTime;) {
+		PIDRegulate();
+		if(debug && (millis()%500==0)) debugger();
+		if(endTime-millis()>=SAMPLETIME) delay(SAMPLETIME);
+		else delay(endTime-millis());
+	}
+}
+
+// to reduce the binary for Atmega168 (16KB), Serial output is disabled
+void Motor::debugger() const {	
+	debug()
+
+#ifdef DEBUG
+	if(!Serial.available()) Serial.begin(Baudrate);
+/*
+	Serial.print("pinPWM -> ");
+	Serial.println(pinPWM,DEC);
+	Serial.print("pinDir -> ");
+	Serial.println(pinDir,DEC);
+	Serial.print("pinIRQ -> ");
+	Serial.println(pinIRQ,DEC);
+	Serial.print("pinIRQB-> ");
+	Serial.println(pinIRQB,DEC);
+ */
+
+	Serial.print("DesiredDir -> ");
+	Serial.println(desiredDirection);
+	Serial.print("currDir ->");
+	Serial.println(isr->currDirection);
+
+	Serial.print("PWM    -> ");
+	Serial.println(speedPWM,DEC);
+	Serial.print("Input  -> ");
+	Serial.println(speedRPMInput,DEC);
+	Serial.print("Output -> ");
+	Serial.println(speedRPMOutput,DEC);
+	Serial.print("Desired-> ");
+	Serial.println(speedRPMDesired,DEC);
+ 
+/*
+	Serial.print("speed2DutyCycle-> ");
+	Serial.println(speed2DutyCycle);
+	Serial.print("speedPPS> ");
+	Serial.println(isr->speedPPS,DEC);
+	Serial.print("pulses -> ");
+	Serial.println(isr->pulses,DEC);
+ */
+
+#endif
+
+}
+
+
+GearedMotor::GearedMotor(unsigned char _pinPWM,unsigned char _pinDir,
+			unsigned char _pinIRQ,unsigned char _pinIRQB,
+			struct ISRVars* _isr,unsigned int ratio):
+				Motor(_pinPWM,_pinDir,_pinIRQ,_pinIRQB,_isr),_ratio(ratio) {
+	debug();
+}
+unsigned int GearedMotor::getRatio() const {
+	return _ratio;
+}
+unsigned int GearedMotor::setRatio(unsigned int ratio) {
+	_ratio=ratio;
+	return getRatio();
+}
+/*
+float GearedMotor::getGearedAccRPMM() const {
+	debug();
+	return (float)Motor::getAccRPMM()/getRatio();
+}
+ */
+float GearedMotor::getGearedSpeedRPM() const {
+	debug();
+	//return (float)Motor::getSpeedRPM()/REDUCTION_RATIO;
+	//return (float)Motor::getSpeedRPM()/_ratio;
+	return (float)Motor::getSpeedRPM()/getRatio();
+}
+float GearedMotor::setGearedSpeedRPM(float gearedSpeedRPM,bool dir) {
+	debug();
+	//Motor::setSpeedRPM(abs(gearedSpeedRPM*REDUCTION_RATIO),dir);
+	Motor::setSpeedRPM(abs(round(gearedSpeedRPM*_ratio)),dir);
+	return getGearedSpeedRPM();
+}
+// direction sensitive, 201208
+float GearedMotor::setGearedSpeedRPM(float gearedSpeedRPM) {
+	debug();
+	//Motor::setSpeedRPM(gearedSpeedRPM*_ratio);
+	Motor::setSpeedRPM(round(gearedSpeedRPM*_ratio));
+	return getGearedSpeedRPM();
+}
+
+float GearedMotor::getPosition() {
+	debug();
+	int ticks_per_rev = getRatio() * CPR;
+	return (float)(Motor::getCurrPulse() % ticks_per_rev)/ticks_per_rev;
+}
+
+MotorWheel::MotorWheel(unsigned char _pinPWM,unsigned char _pinDir,
+						unsigned char _pinIRQ,unsigned char _pinIRQB,
+						struct ISRVars* _isr,
+						unsigned int ratio,unsigned int cirMM):
+						GearedMotor(_pinPWM,_pinDir,_pinIRQ,_pinIRQB,_isr,ratio),_cirMM(cirMM) {
+	debug();
+
+}
+unsigned int MotorWheel::getCirMM() const {
+	return _cirMM;
+}
+unsigned int MotorWheel::setCirMM(unsigned int cirMM) {
+	if(cirMM>0) _cirMM=cirMM;
+	return getCirMM();
+}
+// direction sensitive, 201208
+int MotorWheel::getSpeedCMPM() const {
+	debug();
+	//return int(GearedMotor::getGearedSpeedRPM()*CIR);
+	return int(GearedMotor::getGearedSpeedRPM()*getCirMM()/10);
+}
+int MotorWheel::setSpeedCMPM(unsigned int cm,bool dir) {
+	debug();
+	//GearedMotor::setGearedSpeedRPM(cm/CIR,dir);
+	GearedMotor::setGearedSpeedRPM(cm*10.0/getCirMM(),dir);
+	return getSpeedCMPM();
+}
+// direction sensitive, 201208
+int MotorWheel::setSpeedCMPM(int cm) {
+	debug();
+	//GearedMotor::setGearedSpeedRPM(cm/CIR,dir);
+	GearedMotor::setGearedSpeedRPM(cm*10.0/getCirMM());
+	return getSpeedCMPM();
+}
+
+void MotorWheel::setGearedRPM(float rpm, bool dir) {
+	debug();
+	GearedMotor::setGearedSpeedRPM(rpm, dir);
+	return ;
+}
+// direction sensitive, 201208
+int MotorWheel::getSpeedMMPS() const {
+	debug();
+	return int(getSpeedCMPM()/6);//(mm/sec)/(cm/min) = 6
+}
+
+int MotorWheel::setSpeedMMPS(unsigned int mm,bool dir) {
+	debug();
+	setSpeedCMPM(mm*6,dir);
+	return getSpeedMMPS();
+}
+// direction sensitive, 201208
+int MotorWheel::setSpeedMMPS(int mm) {
+	debug();
+	setSpeedCMPM(mm*6);
+	return getSpeedMMPS();
+}

src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.h

@@ -0,0 +1,349 @@
+
+/*
+V1.1	201104
+1. motorwheel library version 1.1,compatible with maple.
+
+V1.1.1	201110
+1. Add Acceleration
+
+V1.2	201207
+1. Double CPR from 12 to 24, Interrupt Type from RISING to CHANGE.
+2. Reduce default sample time from 10ms to 5ms.
+3. Compatible with Namiki 22CL-3501PG80:1, by "#define _NAMIKI_MOTOR" before "#include ...".
+
+V1.3	201209
+1. R2WD::delayMS(), Omni3WD::delayMS(), Omni4WD::delayMS() are re-implemented, more exactly.
+2. getSpeedRPM(), setSpeedRPM(), getSpeedMMPS(), setSpeedMMPS() are plug-minus/direction sensitive now.
+3. Acceleration is disabled.
+
+V1.4	201209	Not Released
+1. Increase CPR from 24 to 48 for Faulhaber 2342.
+
+V1.5	201209	Omni4WD is re-implemented, and now return value of Omni4WD::getSpeedMMPS() is correct.
+
+ */
+
+/*for maple*/
+#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
+	#include "wirish.h" 
+	#include "./../PID_Beta6/PID_Beta6.h"
+	#include "ext_interrupts.h"
+
+/*for arduino*/	
+#else
+	//#include<Arduino.h>
+	//#include<WProgram.h>
+	#include<PID_Beta6.h>
+	#include<PinChangeInt.h>
+	
+#endif
+
+/*
+
+	class PID
+	class Motor
+	class GearedMotor
+	class MotorWheel
+
+ */
+
+#ifndef MotorWheel_H
+#define MotorWheel_H
+
+#define DIR_ADVANCE HIGH
+#define DIR_BACKOFF LOW
+
+#define  PIN_UNDEFINED 255
+#define  REF_VOLT 12
+/*for maple*/
+#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
+	#define  MAX_PWM  3599
+
+/*for arduino*/	
+#else
+	#define  MAX_PWM 255
+	
+#endif
+
+#ifdef _NAMIKI_MOTOR
+	#define	 TRIGGER CHANGE
+	#define  CPR 4	// Namiki motor
+	#define  DIR_INVERSE
+	#define  REDUCTION_RATIO 80
+#else
+	#define	 TRIGGER CHANGE
+	#define  CPR 24	// Faulhaber motor
+	#define  DIR_INVERSE !
+	#define  REDUCTION_RATIO 64
+#endif
+
+#define  MAX_SPEEDRPM 8000
+//#define  DESIRED_SPEEDRPM 3000
+//#define  SPEED_CONSTANT_RPM 713
+//#define  PWM2SPEED_RESOLUTION 143 //713 * 0.2
+
+#define  SEC_PER_MIN 60
+#define  MICROS_PER_SEC 1000000
+//#define  SPEEDPPS2SPEEDRPM(freq) ((freq)*((SEC_PER_MIN)/(CPR))) //(freq*SEC_PER_MIN/CPR)
+#define  SPEEDPPS2SPEEDRPM(freq) ((unsigned long)(freq)*(SEC_PER_MIN)/(CPR)) //(freq*SEC_PER_MIN/CPR)
+//#define  SPEEDPPS2SPEED2VOLT(spd) ((spd)/SPEED_CONSTANT_RPM)
+//#define  MAX_SPEEDPPS ((MAX_SPEEDRPM*CPR)/SEC_PER_MIN)
+//#define  MIN_SPEEDPPS 0
+
+#define  KC           0.31
+#define  TAUI         0.02
+#define  TAUD         0.00
+#define	 SAMPLETIME	  5
+
+#define Baudrate	19200
+/*
+#ifndef DEBUG
+#define DEBUG
+#endif
+ */
+#ifdef DEBUG
+#define debug() { \
+	if(!Serial.available()) Serial.begin(Baudrate); \
+	Serial.println(__func__);}
+#else
+#define debug() {}
+#endif
+
+
+// Interrupt Type: RISING --> CHANGE	201207
+#define irqISR(y,x) \
+    void x(); \
+    struct ISRVars y={x}; \
+    void x() { \
+        static bool first_pulse=true; \
+        y.pulseEndMicros=micros(); \
+        if(first_pulse==false && y.pulseEndMicros>y.pulseStartMicros) { \
+            y.speedPPS=MICROS_PER_SEC/(y.pulseEndMicros-y.pulseStartMicros); \
+			/* y.accPPSS=(y.speedPPS-y.lastSpeedPPS)*y.speedPPS; */ \
+        } else first_pulse=false; \
+        y.pulseStartMicros=y.pulseEndMicros; \
+		/* y.lastSpeedPPS=y.speedPPS; */ \
+		if(y.pinIRQB!=PIN_UNDEFINED) \
+			y.currDirection=DIR_INVERSE(digitalRead(y.pinIRQ)^digitalRead(y.pinIRQB)); \
+		y.currDirection==DIR_ADVANCE?++y.pulses:--y.pulses; \
+    } 
+
+struct ISRVars {
+	void (*ISRfunc)();
+	//volatile unsigned long pulses;
+	volatile long pulses;	// 201104, direction sensitive
+	volatile unsigned long pulseStartMicros;
+	volatile unsigned long pulseEndMicros;
+	volatile unsigned int  speedPPS;
+	//volatile unsigned int  lastSpeedPPS;
+	//volatile int accPPSS;	// acceleration, Pulse Per Sec^2
+	volatile bool currDirection;
+	unsigned char pinIRQB;
+	unsigned char pinIRQ;	// pinIRQA 201207
+};
+
+class Motor: public PID {
+public:
+	Motor(unsigned char _pinPWM,unsigned char _pinDir,
+			unsigned char _pinIRQ,unsigned char _pinIRQB,
+			struct ISRVars* _isr);
+	
+	void setupInterrupt();
+
+	unsigned char getPinPWM() const;
+	unsigned char getPinDir() const;
+	unsigned char getPinIRQ() const;
+	unsigned char getPinIRQB() const;
+	
+	unsigned int runPWM(unsigned int PWM,bool dir,bool saveDir=true);
+	unsigned int getPWM() const;
+	unsigned int advancePWM(unsigned int PWM);
+	unsigned int backoffPWM(unsigned int PWM);
+
+	bool setDesiredDir(bool dir);
+	bool getDesiredDir() const;
+	bool reverseDesiredDir();
+
+	bool setCurrDir();
+	bool getCurrDir() const;
+
+	//int getAccRPMM() const;		// Acceleration, Round Per Min^2
+	//unsigned int getSpeedRPM() const;
+	//unsigned int setSpeedRPM(int speedRPM,bool dir);
+	// direction sensitive 201208
+	int getSpeedRPM() const;
+	unsigned int setSpeedRPM(int speedRPM,bool dir);	// preserve
+	int setSpeedRPM(int speedRPM);
+	
+	//void simpleRegulate();
+
+	bool PIDSetup(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=1000);
+	bool PIDGetStatus() const;
+	bool PIDEnable(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=1000);
+	bool PIDDisable();
+	bool PIDReset();
+	bool PIDRegulate(bool doRegulate=true);
+	unsigned int PIDSetSpeedRPMDesired(unsigned int speedRPM);
+	unsigned int PIDGetSpeedRPMDesired() const;
+
+	void delayMS(unsigned int ms,bool debug=false);
+	void debugger() const;
+
+	//int getAccPPSS() const;
+	int getSpeedPPS() const;
+	long getCurrPulse() const;
+	long setCurrPulse(long _pulse);
+	long resetCurrPulse();
+	
+	struct ISRVars* isr;
+
+private:
+	unsigned char pinPWM;
+	unsigned char pinDir;
+	//unsigned char pinIRQ;		// moved to isr
+	//unsigned char pinIRQB;
+
+	//bool currDirection;		// current direction
+	bool desiredDirection;	// desired direction
+	unsigned int speedPWM;	// current PWM
+
+	int speedRPMInput;		// RPM: Round Per Minute
+	int speedRPMOutput;		// RPM
+	int speedRPMDesired;	// RPM
+	//float PWMEC;
+	float speed2DutyCycle;
+/*
+	// the followings are defined in struct ISRvars, 
+	// because ISR must be a global function, without parameters and no return value
+
+	volatile unsigned int speedPPS;	// PPS: Pulses Per Second
+	volatile unsigned long pulseStartMicros;
+	volatile unsigned long pulseEndMicros;
+ */
+	bool pidCtrl;
+
+	Motor();
+
+};
+
+
+#ifndef REDUCTION_RATIO
+#define  REDUCTION_RATIO 64
+#endif
+class GearedMotor: public Motor {	// RPM
+public:
+	GearedMotor(unsigned char _pinPWM,unsigned char _pinDir,
+					unsigned char _pinIRQ,unsigned char _pinIRQB,
+					struct ISRVars* _isr,
+					unsigned int _ratio=REDUCTION_RATIO);
+	//float getGearedAccRPMM() const;		// Acceleration, Round Per Min^2
+	float getGearedSpeedRPM() const;
+	float setGearedSpeedRPM(float gearedSpeedRPM,bool dir);
+	// direction sensitive 201208
+	float setGearedSpeedRPM(float gearedSpeedRPM);
+	unsigned int getRatio() const;
+	unsigned int setRatio(unsigned int ratio=REDUCTION_RATIO);
+	float getPosition();
+private:
+	unsigned int _ratio;
+};
+
+
+#ifndef PI
+#define PI 3.1416
+#endif
+//#define CIR 31.4	// cm
+//#define CIRMM 150	// mm
+//#define CIRMM 188
+#define CIRMM 314
+class MotorWheel: public GearedMotor {	// 
+public:
+	MotorWheel(unsigned char _pinPWM,unsigned char _pinDir,
+				unsigned char _pinIRQ,unsigned char _pinIRQB,
+				struct ISRVars* _isr,
+				unsigned int ratio=REDUCTION_RATIO,unsigned int cirMM=CIRMM);
+
+	unsigned int getCirMM() const;
+	unsigned int setCirMM(unsigned int cirMM=CIRMM);
+/*
+	int getAccCMPMM() const;	// Acceleration, CM Per Min^2
+	unsigned int getSpeedCMPM() const;	// cm/min
+	unsigned int setSpeedCMPM(unsigned int cm,bool dir);
+	int getAccMMPSS() const;	// Acceleration, MM Per Sec^2
+	unsigned int getSpeedMMPS() const; // mm/s
+	unsigned int setSpeedMMPS(unsigned int mm,bool dir);
+ */
+	// direction sensitive 201208
+	//int getAccCMPMM() const;	// Acceleration, CM Per Min^2
+	int getSpeedCMPM() const;	// cm/min
+	int setSpeedCMPM(unsigned int cm,bool dir);	// preserve
+	int setSpeedCMPM(int cm);
+	//int getAccMMPSS() const;	// Acceleration, MM Per Sec^2
+	int getSpeedMMPS() const; // mm/s
+	int setSpeedMMPS(unsigned int mm,bool dir); // preserve
+	int setSpeedMMPS(int mm);
+	void setGearedRPM(float rpm, bool dir);
+
+	//unsigned int runTime(unsigned int speedMMPS,bool dir,unsigned int TimeMS);
+	//unsigned int runDistance(unsigned int speedMMPS,bool dir,unsigned int distanceMM);
+
+private:
+	unsigned int _cirMM;
+};
+
+/*
+class samePID: public PID {
+public:
+	samePID(int* input,int* output,int* setPoint,float kc,float taui,float taud)
+	:PID(input,output,setPoint,kc,taui,taud) { }
+};
+
+class servoMotor: public Motor,public samePID {
+public:
+	servoMotor(unsigned char _pinPWM,unsigned char _pinDir,
+			unsigned char _pinIRQ,unsigned char _pinIRQB,
+			struct ISRVars* _isr);
+
+	int SPIDSetPulseBoundary(int min,int max);
+	bool SPIDSetup(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=10);
+	bool PIDRegulate(bool doRegulate=true);
+
+	enum {
+		SPIDMODE_UNKNOWN,
+		SPIDMODE_SPEED,
+		SPIDMODE_STOP,
+		SPIDMODE_PULSE,
+	};
+	unsigned char getSPIDMode() const;
+	unsigned char setSPIDMode(unsigned char _mode);
+
+	int getPulseDesired() const;
+	int setPulseDesired(int _pulse);
+	int incPulseDesired();
+	int decPulseDesired();
+
+	bool backToZero();
+	bool scanZero();
+
+	void debugger() const;
+
+private:
+	int pulseInput;		//
+	int pulseOutput;	//
+	int pulseDesired;	//
+
+	int pulse2SpeedRPM;
+
+	unsigned char SPIDMode;
+
+	bool SPIDRegulateSpeed(bool doRegulate=true);	// 201104
+	bool SPIDRegulateStop(bool doRegulate=true);
+	bool SPIDRegulatePulse(bool doRegulate=true);
+
+};
+ */
+
+
+
+#endif
+

src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample/PIDSample.pde

@@ -0,0 +1,31 @@
+/********************************************************
+ * PID Simple Example
+ * Reading analog input 0 to control analog PWM output 3
+ ********************************************************/
+
+#include <PID_Beta6.h>
+
+//Define Variables we'll be connecting to
+double Setpoint, Input, Output;
+
+//Specify the links and initial tuning parameters
+PID myPID(&Input, &Output, &Setpoint,2,5,1);
+
+void setup()
+{
+  //initialize the variables we're linked to
+  Input = analogRead(0);
+  Setpoint = 100;
+
+  //turn the PID on
+  myPID.SetMode(AUTO);
+}
+
+void loop()
+{
+  Input = analogRead(0);
+  myPID.Compute();
+  analogWrite(3,Output);
+}
+
+

src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample2/PIDSample2.pde

@@ -0,0 +1,55 @@
+/***************************************************
+ * Advanced PID Example
+ * PID input: Analog input 0
+ * output = time proportion on/off of output 0 (50% = 2 sec on, 2 sec off)
+ * and just to make it more complicated:
+ * - start with a setpoint ramp (0 to 500 over 5 minutes (300 seconds))
+ * - use gap control (moderate tunings when within 100 of setpoint, aggressive when outside 100)
+ ***************************************************/
+ 
+ #include <PID_Beta6.h>
+ 
+ unsigned long windowStartTime;
+ unsigned long RampStartTime;
+ double Input, Output, Setpoint;
+ PID pid(&Input, &Output, &Setpoint, 3,4,1);
+ 
+unsigned long printTime= millis();
+ 
+ void setup()
+ {
+  
+   pid.SetOutputLimits(0,4000); //tell the PID to range the output from 0 to 4000 
+   Output = 4000; //start the output at its max and let the PID adjust it from there
+  
+   pid.SetMode(AUTO); //turn on the PID
+   windowStartTime = millis();
+   RampStartTime = millis();
+ }
+ 
+ 
+ void loop()
+ {
+
+  //Set Point Ramp
+   if(millis()-RampStartTime<300000)
+   {
+     Setpoint = ((double)(millis()-RampStartTime))/(300000.0)*500.0;
+   }
+   else Setpoint = 500;
+   
+   //Set Tuning Parameters based on how close we are to setpoint
+   if(abs(Setpoint-Input)>100)pid.SetTunings(6,4,1);  //aggressive
+   else pid.SetTunings(3,4,1); //comparatively moderate
+   
+   //give the PID the opportunity to compute if needed
+   Input = analogRead(0);
+   pid.Compute();
+   
+   //turn the output pin on/off based on pid output
+   if(millis()-windowStartTime>4000) windowStartTime+=4000;
+   if(Output > millis()-windowStartTime) digitalWrite(0,HIGH);
+   else digitalWrite(0,LOW);
+
+ }
+

src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample3/PIDSample3.pde

@@ -0,0 +1,54 @@
+#include <fuzzy_table.h>
+#include <PID_Beta6.h>
+
+/***************************************************
+ * Feed Forward Example : RepRap Extruder Nozzle Temperature
+ * PID input: Nozzle Temperature
+ * PID output: PWM signal (0-255) to the heater control
+ * Other Input: Plastic Feed rate is being read on input 1
+ * For the feed forward piece...
+ *  The amount of heat we want to add to the nozzle is largely dependent
+ *  on how fast plastic is being fed.  if the feed is stopped we're going
+ *  to want a lot less heat than when the system is running.  A pid 
+ *  control by itself will eventually adjust the heat, but by then the temp
+ *  will be really high.  feed forward can help with this.
+ *    so to make Feed Forward work, at every cycle we look at the current feed speed
+ *  and compute a "bias" which we feed to the pid controller.  sort of: "Based on my
+ *  knowledge, your output should be about X.  adjust off of that as you see fit"
+ *    What does it get you?  well, the instant the feed stops, the baseline drops. That
+ *  means that the controller output immediately drops. if you had been using just a pid, 
+ *  you'd have to wait for the temperature to rise quite a bit before seeing the same
+ *  drop in output.
+ * 
+ * (for this example, we're assuming that for a feed rate of 1023 the output should be
+ *  about 130, and at a rate of 0 the output should be around 10)
+ ***************************************************/
+ 
+ #include <PID_Beta6.h>
+ int FeedRate;
+ 
+ double Input, Output, Setpoint, Bias;
+ PID pid(&Input, &Output, &Setpoint, &Bias, 3, 4, 1);
+ 
+ void setup()
+ {
+   Setpoint = 400;
+   pid.SetMode(AUTO); //turn on the PID
+
+ }
+ 
+ 
+ void loop()
+ {
+
+   //Read in the Feed rate and compute a baseline output to the heater
+   FeedRate = analogRead(1);  //read the feed rate
+   Bias = (FeedRate/1023)*(120)+10;
+   
+   //give the PID the opportunity to compute if needed
+   Input = analogRead(0);
+   pid.Compute();
+   
+   analogWrite(0, Output);
+
+ }

src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.cpp

@@ -0,0 +1,309 @@
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "wiring.h"
+#endif
+
+#include <PID_Beta6.h>
+#include "fuzzy_table.h"
+#include <wiring_private.h>
+#include <HardwareSerial.h>
+
+/* Standard Constructor (...)***********************************************
+ *    constructor used by most users.  the parameters specified are those for
+ * for which we can't set up reliable defaults, so we need to have the user
+ * set them.
+ ***************************************************************************/ 
+PID::PID(int *Input, int *Output, int *Setpoint, float Kc, float TauI, float TauD)
+{
+
+  PID::ConstructorCommon(Input, Output, Setpoint, Kc, TauI, TauD);  
+  UsingFeedForward = false;
+  PID::Reset();
+
+  
+}
+
+/* Overloaded Constructor(...)**********************************************
+ *    This one is for more advanced users.  it's essentially the same as the
+ * standard constructor, with one addition.  you can link to a Feed Forward bias,
+ * which lets you implement... um.. Feed Forward Control.  good stuff.
+ ***************************************************************************/
+PID::PID(int *Input, int *Output, int *Setpoint, int *FFBias, float Kc, float TauI, float TauD)
+{
+
+  PID::ConstructorCommon(Input, Output, Setpoint, Kc, TauI, TauD);  
+  UsingFeedForward = true;			  //tell the controller that we'll be using an external
+  myBias = FFBias;                              //bias, and where to find it
+  PID::Reset();
+
+}
+
+/* ConstructorCommon(...)****************************************************
+ *    Most of what is done in the two constructors is the same.  that code
+ * was put here for ease of maintenance and (minor) reduction of library size
+ ****************************************************************************/
+void PID::ConstructorCommon(int *Input, int *Output, int *Setpoint, float Kc, float TauI, float TauD)
+{
+  PID::SetInputLimits(0, 1023);		//default the limits to the 
+  PID::SetOutputLimits(0, 255);		//full ranges of the I/O
+
+  tSample = 1000;			//default Controller Sample Time is 1 second
+
+  PID::SetTunings( Kc, TauI, TauD);
+
+  nextCompTime = millis();
+  inAuto = false;
+  myOutput = Output;
+  myInput = Input;
+  mySetpoint = Setpoint;
+
+  Err = lastErr = prevErr = 0;
+}
+
+
+/* SetInputLimits(...)*****************************************************
+ *	I don't see this function being called all that much (other than from the
+ *  constructor.)  it needs to be here so we can tell the controller what it's
+ *  input limits are, and in most cases the 0-1023 default should be fine.  if
+ *  there's an application where the signal being fed to the controller is
+ *  outside that range, well, then this function's here for you.
+ **************************************************************************/
+void PID::SetInputLimits(int INMin, int INMax)
+{
+	//after verifying that mins are smaller than maxes, set the values
+	if(INMin >= INMax) return;
+	
+	
+	inMin = INMin;
+	inSpan = INMax - INMin;
+}
+
+/* SetOutputLimits(...)****************************************************
+ *     This function will be used far more often than SetInputLimits.  while
+ *  the input to the controller will generally be in the 0-1023 range (which is
+ *  the default already,)  the output will be a little different.  maybe they'll
+ *  be doing a time window and will need 0-8000 or something.  or maybe they'll
+ *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
+ *  here.
+ **************************************************************************/
+void PID::SetOutputLimits(int OUTMin, int OUTMax)
+{
+	//after verifying that mins are smaller than maxes, set the values
+	if(OUTMin >= OUTMax) return;
+
+	outMin = OUTMin;
+	outSpan = OUTMax - OUTMin;
+}
+
+/* SetTunings(...)*************************************************************
+ * This function allows the controller's dynamic performance to be adjusted. 
+ * it's called automatically from the constructor, but tunings can also
+ * be adjusted on the fly during normal operation
+ ******************************************************************************/
+void PID::SetTunings(float Kc, float TauI, float TauD)
+{
+	//verify that the tunings make sense
+	if (Kc == 0.0 || TauI < 0.0 || TauD < 0.0) return;
+
+	//we're going to do some funky things to the input numbers so all
+	//our math works out, but we want to store the numbers intact
+	//so we can return them to the user when asked.
+	P_Param = Kc;
+	I_Param = TauI;
+	D_Param = TauD;
+
+	//convert Reset Time into Reset Rate, and compensate for Calculation frequency
+	float tSampleInSec = ((float)tSample / 1000.0);
+	float tempTauR;
+	if (TauI == 0.0) 
+		tempTauR = 0.0;
+	else 
+		tempTauR = (1.0 / TauI) * tSampleInSec;
+
+	
+	kc = Kc;
+	taur = tempTauR;
+	taud = TauD / tSampleInSec;
+
+	cof_A = kc * (1 + taur + taud);
+	cof_B = kc * (1 + 2 * taud);
+	cof_C = kc * taud;
+}
+
+/* Reset()*********************************************************************
+ *	does all the things that need to happen to ensure a bumpless transfer
+ *  from manual to automatic mode.  this shouldn't have to be called from the
+ *  outside. In practice though, it is sometimes helpful to start from scratch,
+ *  so it was made publicly available
+ ******************************************************************************/
+void PID::Reset()
+{
+
+	if(UsingFeedForward)
+	  bias = (*myBias - outMin) / outSpan;
+	else
+	  bias = (*myOutput - outMin) / outSpan;
+
+}
+
+/* SetMode(...)****************************************************************
+ * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
+ * when the transition from manual to auto occurs, the controller is
+ * automatically initialized
+ ******************************************************************************/
+void PID::SetMode(int Mode)
+{
+	if (Mode!=0 && !inAuto)
+	{	//we were in manual, and we just got set to auto.
+		//reset the controller internals
+		PID::Reset();
+	}
+	inAuto = (Mode!=0);
+}
+
+/* SetSampleTime(...)*******************************************************
+ * sets the frequency, in Milliseconds, with which the PID calculation is performed	
+ ******************************************************************************/
+void PID::SetSampleTime(int NewSampleTime)
+{
+	if (NewSampleTime > 0)
+	{ 
+		//convert the time-based tunings to reflect this change
+		taur *= ((float)NewSampleTime)/((float) tSample);
+		taud *= ((float)NewSampleTime)/((float) tSample);
+		tSample = (unsigned long)NewSampleTime;
+
+		cof_A = kc * (1 + taur + taud);
+		cof_B = kc * (1 + 2 * taud);
+		cof_C = kc * taud;
+	}
+}
+
+/* Compute() **********************************************************************
+ *     This, as they say, is where the magic happens.  this function should be called
+ *   every time "void loop()" executes.  the function will decide for itself whether a new
+ *   pid Output needs to be computed
+ *
+ *  Some notes for people familiar with the nuts and bolts of PID control:
+ *  - I used the Ideal form of the PID equation.  mainly because I like IMC
+ *    tunings.  lock in the I and D, and then just vary P to get more 
+ *    aggressive or conservative
+ *
+ *  - While this controller presented to the outside world as being a Reset Time
+ *    controller, when the user enters their tunings the I term is converted to
+ *    Reset Rate.  I did this merely to avoid the div0 error when the user wants
+ *    to turn Integral action off.
+ *    
+ *  - Derivative on Measurement is being used instead of Derivative on Error.  The
+ *    performance is identical, with one notable exception.  DonE causes a kick in
+ *    the controller output whenever there's a setpoint change. DonM does not.
+ *
+ *  If none of the above made sense to you, and you would like it to, go to:
+ *  http://www.controlguru.com .  Dr. Cooper was my controls professor, and is
+ *  gifted at concisely and clearly explaining PID control
+ *********************************************************************************/
+void PID::Compute()
+{
+	justCalced=false;
+	if (!inAuto) return; //if we're in manual just leave;
+
+	unsigned long now = millis();
+
+	//millis() wraps around to 0 at some point.  depending on the version of the 
+	//Arduino Program you are using, it could be in 9 hours or 50 days.
+	//this is not currently addressed by this algorithm.
+	
+									
+	//...Perform PID Computations if it's time...
+	if (now>=nextCompTime)							
+	{
+  
+		Err = *mySetpoint - *myInput;
+		//if we're using an external bias (i.e. the user used the 
+		//overloaded constructor,) then pull that in now
+		if(UsingFeedForward)
+		{
+			bias = *myBias - outMin;
+		}
+
+
+		// perform the PID calculation.  
+		//float output = bias + kc * ((Err - lastErr)+ (taur * Err) + (taud * (Err - 2*lastErr + prevErr)));
+		noInterrupts();
+		int output = bias + (cof_A * Err - cof_B * lastErr + cof_C * prevErr);
+		interrupts();
+
+		//make sure the computed output is within output constraints
+		if (output < -outSpan) output = -outSpan;
+		else if (output > outSpan) output = outSpan;
+		
+
+		prevErr = lastErr;
+		lastErr = Err;
+
+
+		//scale the output from percent span back out to a real world number
+						*myOutput = output;
+
+		nextCompTime += tSample;				// determine the next time the computation
+		if(nextCompTime < now) nextCompTime = now + tSample;	// should be performed	
+
+		justCalced=true;  //set the flag that will tell the outside world that the output was just computed
+
+	}								
+
+
+}
+
+
+/*****************************************************************************
+ * STATUS SECTION
+ * These functions allow the outside world to query the status of the PID
+ *****************************************************************************/
+
+bool PID::JustCalculated()
+{
+	return justCalced;
+}
+int PID::GetMode()
+{
+	if(inAuto)return 1;
+	else return 0;
+}
+
+int PID::GetINMin()
+{
+	return inMin;
+}
+int PID::GetINMax()
+{
+	return inMin + inSpan;
+}
+int PID::GetOUTMin()
+{
+	return outMin;
+}
+int PID::GetOUTMax()
+{
+	return outMin+outSpan;
+}
+int PID::GetSampleTime()
+{
+	return tSample;
+}
+float PID::GetP_Param()
+{
+	return P_Param;
+}
+float PID::GetI_Param()
+{
+	return I_Param;
+}
+
+float PID::GetD_Param()
+{
+	return D_Param;
+}
+

src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.h

@@ -0,0 +1,113 @@
+#ifndef PID_Beta6_h
+#define PID_Beta6_h
+
+class PID
+{
+
+
+  public:
+
+  #define AUTO	1
+  #define MANUAL	0
+  #define LIBRARY_VERSION	0.6
+
+  //commonly used functions **************************************************************************
+    PID(int*, int*, int*,				// * constructor.  links the PID to the Input, Output, and 
+        float, float, float);			//   Setpoint.  Initial tuning parameters are also set here
+
+    PID(int*, int*, int*,				// * Overloaded Constructor.  if the user wants to implement
+        int*, float, float, float);		//   feed-forward
+
+    void SetMode(int Mode);			// * sets PID to either Manual (0) or Auto (non-0)
+
+    void Compute();					// * performs the PID calculation.  it should be
+										//   called every time loop() cycles. ON/OFF and
+										//   calculation frequency can be set using SetMode
+										//   SetSampleTime respectively
+
+    void SetInputLimits(int, int);  //Tells the PID what 0-100% are for the Input
+
+    void SetOutputLimits(int, int); //Tells the PID what 0-100% are for the Output
+
+
+  //available but not commonly used functions ********************************************************
+    void SetTunings(float, float,	// * While most users will set the tunings once in the 
+                    float);				//   constructor, this function gives the user the option
+										//   of changing tunings during runtime for Adaptive control
+
+    void SetSampleTime(int);		// * sets the frequency, in Milliseconds, with which 
+										//   the PID calculation is performed.  default is 1000 
+
+    void Reset();						// * reinitializes controller internals.  automatically
+										//   called on a manual to auto transition
+
+    bool JustCalculated();			// * in certain situations, it helps to know when the PID has
+										//   computed this bit will be true for one cycle after the
+										//   pid calculation has occurred
+    
+
+   //Status functions allow you to query current PID constants ***************************************
+    int GetMode();
+    int GetINMin();
+    int GetINMax();
+    int GetOUTMin();
+    int GetOUTMax();
+    int GetSampleTime();
+    float GetP_Param();
+    float GetI_Param();
+    float GetD_Param();
+
+
+  private:
+
+    void ConstructorCommon(int*, int*, int*,           // * code that is shared by the constructors
+        float, float, float);
+
+   //scaled, tweaked parameters we'll actually be using
+    float kc;                    // * (P)roportional Tuning Parameter
+    float taur;                  // * (I)ntegral Tuning Parameter
+    float taud;                  // * (D)erivative Tuning Parameter
+
+	float cof_A;
+	float cof_B;
+	float cof_C;
+
+   //nice, pretty parameters we'll give back to the user if they ask what the tunings are
+    float P_Param;
+    float I_Param;
+    float D_Param;
+
+
+    int *myInput;				// * Pointers to the Input, Output, and Setpoint variables
+    int *myOutput;				//   This creates a hard link between the variables and the 
+    int *mySetpoint;			//   PID, freeing the user from having to constantly tell us
+								//   what these values are.  with pointers we'll just know.
+
+    int *myBias;				// * Pointer to the External FeedForward bias, only used 
+								//   if the advanced constructor is used
+    bool UsingFeedForward;		// * internal flag that tells us if we're using FeedForward or not
+
+    unsigned long nextCompTime;    // * Helps us figure out when the PID Calculation needs to
+                                  //   be performed next
+                                  //   to determine when to compute next
+    unsigned long tSample;       // * the frequency, in milliseconds, with which we want the
+                                  //   the PID calculation to occur.
+    bool inAuto;                  // * Flag letting us know if we are in Automatic or not
+
+                                  //   the derivative required for the D term
+    //float accError;              // * the (I)ntegral term is based on the sum of error over
+                                  //   time.  this variable keeps track of that
+    float bias;                  // * the base output from which the PID operates
+    
+	int Err;
+	int lastErr;
+	int prevErr;
+	
+    float inMin, inSpan;         // * input and output limits, and spans.  used convert
+    float outMin, outSpan;       //   real world numbers into percent span, with which
+								 //   the PID algorithm is more comfortable.
+
+    bool justCalced;			// * flag gets set for one cycle after the pid calculates
+};
+#endif
+

src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h

@@ -0,0 +1,66 @@
+
+#if 0
+#ifndef	_PID_TABLE_H_
+#define	_PID_TABLE_H_
+
+#define	PB				6
+#define	PM				5
+#define	PS				4
+#define	ZO				3
+#define	NS				2
+#define	NM				1
+#define	NB				0
+
+#define	DELTA_KP_IDX	0
+#define	DELATA_KI_IDX	1
+#define	ERR				7
+#define	DELTA_ERR		7
+
+
+const char pid_tbl[2][7][7]={
+						{
+							{PB,PB,PM,PM,PS,ZO,ZO},
+							{PB,PB,PM,PS,PS,ZO,ZO},
+							{PM,PM,PM,PS,ZO,NS,NS},
+							{PM,PM,PS,ZO,NS,NM,NM},
+							{PS,PS,ZO,NS,NS,NM,NM},
+							{PS,ZO,NS,NM,NM,NM,NB},
+							{ZO,ZO,NM,NM,NM,NB,NB},
+						},
+						
+						{
+							{NB,NB,NM,NM,NS,ZO,ZO},
+							{NB,NB,NM,NS,NS,ZO,ZO},
+							{NB,NM,NS,NS,ZO,PS,PS},
+							{NM,NM,NS,ZO,PS,PM,PM},
+							{NM,NS,ZO,PS,PS,PM,PB},
+							{ZO,ZO,PS,PS,PM,PB,PB},
+							{ZO,ZO,PS,PM,PM,PB,PB},
+						},
+					 };
+
+const float kp_tbl[ERR] = {
+						0.07987,
+						0.08974,
+						0.09948,
+						0.12922,
+						0.54896,
+						0.70000,
+						0.76000,
+				    };
+
+
+const float ki_tbl[DELTA_ERR] = {
+							0.02451,
+							0.07353,
+							0.14706,
+							0.19608,
+							0.24510,
+							0.30000,
+							0.00000,
+						   };
+ 
+#endif	_PID_TABLE_H_
+
+#endif
+

src/dcaitirobot/embedded/lib/PID_Beta6/keywords.txt

@@ -0,0 +1,38 @@
+#######################################
+# Syntax Coloring Map For LiquidCrystal
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+PID	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+SetMode	KEYWORD2
+Compute	KEYWORD2
+SetInputLimits	KEYWORD2
+SetOutputLimits	KEYWORD2
+SetTunings	KEYWORD2
+SetSampleTime	KEYWORD2
+JustCalculated	KEYWORD2
+Reset	KEYWORD2
+GetMode	KEYWORD2
+GetINMin	KEYWORD2
+GetINMax	KEYWORD2
+GetOUTMin	KEYWORD2
+GetOUTMax	KEYWORD2
+GetSampleTime	KEYWORD2
+GetP_Param	KEYWORD2
+GetI_Param	KEYWORD2
+GetD_Param	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
+AUTO	LITERAL1
+MANUAL 	LITERAL1

src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.cpp

@@ -0,0 +1,191 @@
+/*
+	PinChangeInt.cpp
+*/
+
+#include <PinChangeInt.h>
+
+
+PCintPort::PCintPin PCintPort::PCintPin::pinDataAlloc[MAX_PIN_CHANGE_PINS];
+
+
+PCintPort PCintPort::pcIntPorts[] = {
+	PCintPort(0,PCMSK0),
+	PCintPort(1,PCMSK1),
+	PCintPort(2,PCMSK2)
+};
+
+void PCintPort::addPin(uint8_t mode,uint8_t mask,PCIntvoidFuncPtr userFunc)
+{
+	//int i = 0;
+	PCintPin* p = PCintPin::pinDataAlloc;
+	do {
+		if (!p->PCintMode) { // found an available pin data structure
+			// now find the next slot to put it in
+			PCintPin** t = &pcIntPins[0];
+			do {
+				if (!*t) {	// we have a slot
+					// fill in the data
+					p->PCintMode = mode;
+					*t = p;
+					p->PCintFunc = userFunc;
+					// set the mask
+					pcmask |= p->PCIntMask = mask;
+					// enable the interrupt
+					PCICR |= PCICRbit;
+
+				#ifdef	DEBUG
+					Serial.print("BitMask = ");
+					Serial.print(mask, BIN);
+					Serial.print("; ");
+					Serial.print("slot = ");
+					Serial.println((int)(t - &pcIntPins[0]), DEC);
+				#endif
+				
+					return;
+				}
+			}	 while (int(++t) < int(&pcIntPins[8]));
+		}
+	} while (int(++p) < int(&PCintPin::pinDataAlloc[MAX_PIN_CHANGE_PINS]));
+}
+
+void PCintPort::delPin(uint8_t mask)
+{
+	PCintPin** t = pcIntPins;
+	while (*t) {
+		PCintPin& p = **t;
+		if (p.PCIntMask == mask) {	// found the target
+			uint8_t oldSREG = SREG;
+			cli();
+			// disable the mask.
+			pcmask &= ~mask;
+			// if that's the last one, disable the interrupt.
+			if (pcmask == 0) {
+				PCICR &= ~(PCICRbit);
+			}
+			p.PCIntMask = 0;
+			p.PCintMode = 0;
+			p.PCintFunc = NULL;
+			do { // shuffle down as we pass through the list, filling the hole
+				*t = t[1];
+			} while (*t);
+			SREG = oldSREG;
+			return;
+		}
+		t++;
+	}
+}
+
+/*
+ * attach an interrupt to a specific pin using pin change interrupts.
+ */
+void PCintPort::attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode)
+{
+	uint8_t portNum = digitalPinToPort(pin);
+	#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+		if (((portNum != 2) && (portNum != 11)) 
+			|| (userFunc == NULL)
+			) {
+			
+		#ifdef	DEBUG
+			Serial.println("NOT_A_PORT det");
+		#endif
+		
+			return;
+		}
+		// map pin to PCIR register
+		uint8_t portIndex = (portNum == 2)?(0):(2);
+	#else
+		if ((portNum == NOT_A_PORT) || (userFunc == NULL)) {
+			return;
+		}
+		// map pin to PCIR register
+		uint8_t portIndex = portNum - 2;
+	#endif
+
+	#ifdef	DEBUG
+		Serial.print("portNum = ");
+		Serial.print(portNum, DEC);
+		Serial.print("; ");
+	#endif
+	
+	PCintPort &port = PCintPort::pcIntPorts[portIndex];
+	port.addPin(mode,digitalPinToBitMask(pin),userFunc);
+}
+
+void PCintPort::detachInterrupt(uint8_t pin)
+{
+	uint8_t portNum = digitalPinToPort(pin);
+	#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+		if ((portNum != 2) && (portNum != 11)) {
+			//Serial.println("NOT_A_PORT det");
+			return;
+		}
+		uint8_t portIndex = (portNum == 2)?(0):(2);
+		PCintPort& port = PCintPort::pcIntPorts[portIndex];
+
+	#else
+		if (portNum == NOT_A_PORT) {
+			//Serial.println("NOT_A_PORT det");
+			return;
+		}
+		PCintPort& port = PCintPort::pcIntPorts[portNum - 2];
+	#endif
+	
+	port.delPin(digitalPinToBitMask(pin));
+}
+
+// common code for isr handler. "port" is the PCINT number.
+// there isn't really a good way to back-map ports and masks to pins.
+void PCintPort::PCint() {
+	#ifndef DISABLE_PCINT_MULTI_SERVICE
+	uint8_t pcifr;
+	do {
+	#endif
+		uint8_t curr = portInputReg;
+		uint8_t mask = curr ^ PCintLast;
+		// get the pin states for the indicated port.
+		// mask is pins that have changed. screen out non pcint pins.
+		if ((mask &= pcmask) == 0) {
+			return;
+		}
+		PCintLast = curr;
+		PCintPin** t = pcIntPins;
+		while (*t) {
+			PCintPin& p = **t;
+			if (p.PCIntMask & mask) { // a changed bit
+				// Trigger interrupt if mode is CHANGE, or if mode is RISING and
+				// the bit is currently high, or if mode is FALLING and bit is low.
+				if (p.PCintMode == CHANGE
+					|| ((p.PCintMode == FALLING) && !(curr & p.PCIntMask))
+					|| ((p.PCintMode == RISING) && (curr & p.PCIntMask))
+					) {
+					p.PCintFunc();
+				}
+			}
+			t++;
+		}
+	#ifndef DISABLE_PCINT_MULTI_SERVICE
+		pcifr = PCIFR & PCICRbit;
+		PCIFR = pcifr;	// clear the interrupt if we will process is (no effect if bit is zero)
+	} while(pcifr);
+	#endif
+}
+
+#ifndef NO_PORTB_PINCHANGES
+ISR(PCINT0_vect) {
+	PCintPort::pcIntPorts[0].PCint();
+}
+#endif
+
+#ifndef NO_PORTC_PINCHANGES
+ISR(PCINT1_vect) {
+	PCintPort::pcIntPorts[1].PCint();
+}
+#endif
+
+#ifndef NO_PORTD_PINCHANGES
+ISR(PCINT2_vect) {
+	PCintPort::pcIntPorts[2].PCint();
+}
+#endif
+

src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.h

@@ -0,0 +1,136 @@
+/*
+	PinChangeInt.h
+*/
+
+
+#ifndef PinChangeInt_h
+#define	PinChangeInt_h
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+#include "PinChangeIntConfig.h"
+#ifndef Pins_Arduino_h
+#include "pins_arduino.h"
+#endif
+// This library was inspired by and derived from "johnboiles" (it seems) Main.PCInt Arduino Playground example
+// see: http://www.arduino.cc/playground/Main/PcInt
+
+
+// Interrupt management for PCI
+/*
+ * an extension to the interrupt support for arduino.
+ * add pin change interrupts to the external interrupts, giving a way
+ * for users to have interrupts drive off of any pin.
+ * Refer to avr-gcc header files, arduino source and atmega datasheet.
+ */
+
+/*
+ * Theory: all IO pins on AtmegaX(168/328/1280/2560) are covered by Pin Change Interrupts.
+ * The PCINT corresponding to the pin must be enabled and masked, and
+ * an ISR routine provided.  Since PCINTs are per port, not per pin, the ISR
+ * must use some logic to actually implement a per-pin interrupt service.
+ */
+
+/* Pin to interrupt map ON ATmega168/328:
+ * D0-D7 = PCINT 16-23 = PCIR2 = PD = PCIE2 = pcmsk2
+ * D8-D13 = PCINT 0-5 = PCIR0 = PB = PCIE0 = pcmsk0
+ * A0-A5 (D14-D19) = PCINT 8-13 = PCIR1 = PC = PCIE1 = pcmsk1
+ */
+
+
+/* Pin to interrupt map ON ATmega1280/2560:
+ * D50-D53 = PCINT 3-0 = PCIR0 = PB = PCIE0 = pcmsk0
+ * D10-D13 = PCINT 4-7 = PCIR0 = PB = PCIE0 = pcmsk0
+ * A8-A15 (D62-D69) = PCINT 16-23 = PCIR2 = Pk = PCIE2 = pcmsk2
+ 
+  * *******D0 = PCINT 8 = PCIR1 = PE = PCIE1 = pcmsk1**********
+ * *******D14-D15 = PCINT 10-9 = PJ = PCIE1 = pcmsk1*********
+ ********NOTE:PCINT 8-15 does NOT available in this library******
+ */
+
+
+/*
+	Please make any configuration changes in the accompanying PinChangeIntConfig.h file.
+	This will help avoid having to reset your config in the event of changes to the 
+	library code (just don't replace that file when you update).
+
+*/
+
+
+#ifndef MAX_PIN_CHANGE_PINS
+#define	MAX_PIN_CHANGE_PINS 8
+#endif
+
+// You can reduce the memory footprint of this handler by declaring that there will be no pin change interrupts
+// on any of the three ports.
+// define NO_PORTB_PINCHANGES to indicate that port b will not be used for pin change interrupts
+// define NO_PORTC_PINCHANGES to indicate that port c will not be used for pin change interrupts
+// define NO_PORTD_PINCHANGES to indicate that port d will not be used for pin change interrupts
+// If only a single port remains, the handler will be declared inline reducing the size and latency 
+// of the handler.
+
+// if their is only one PCInt vector in use the code can be inlined
+// reducing latecncy and code size
+#define	INLINE_PCINT
+#if ((defined(NO_PORTB_PINCHANGES) && defined(NO_PORTC_PINCHANGES)) || \
+	(defined(NO_PORTC_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) || \
+	(defined(NO_PORTD_PINCHANGES) && defined(NO_PORTB_PINCHANGES)))
+#undef INLINE_PCINT
+#define	INLINE_PCINT inline
+#endif
+
+// Provide drop in compatibility with johnboiles PCInt project at
+// http://www.arduino.cc/playground/Main/PcInt
+#define	PCdetachInterrupt(pin)	PCintPort::detachInterrupt(pin)
+#define	PCattachInterrupt(pin,userFunc,mode) PCintPort::attachInterrupt(pin, userFunc,mode)
+
+
+
+typedef void (*PCIntvoidFuncPtr)(void);
+
+class PCintPort {
+				PCintPort(int index,volatile uint8_t& maskReg) :
+					#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+						portInputReg(*portInputRegister((index == 0)?(2):(index + 9))),
+
+					#else
+						portInputReg(*portInputRegister(index + 2)),
+					#endif
+					
+					pcmask(maskReg),
+					PCICRbit(1 << index),
+					PCintLast(0)	{
+					for (int i = 0; i < 9; i++) {
+						pcIntPins[i] = NULL;
+					}
+				}
+public:
+	static		void attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode);
+	static		void detachInterrupt(uint8_t pin);
+	INLINE_PCINT void PCint();
+	static		PCintPort	pcIntPorts[];
+	
+protected:
+	class PCintPin {
+	public:
+					PCintPin() :
+						PCintFunc((PCIntvoidFuncPtr)NULL),
+						PCintMode(0) {}
+		PCIntvoidFuncPtr PCintFunc;
+		uint8_t 	PCintMode;
+		uint8_t		PCIntMask;
+		static PCintPin	pinDataAlloc[MAX_PIN_CHANGE_PINS];
+	};
+	void		addPin(uint8_t mode,uint8_t mask,PCIntvoidFuncPtr userFunc);
+	void		delPin(uint8_t mask);
+	volatile	uint8_t&		portInputReg;
+	volatile	uint8_t&		pcmask;
+	const		uint8_t			PCICRbit;
+				uint8_t			PCintLast;
+				PCintPin*	pcIntPins[9];	// extra entry is a barrier
+};
+#endif

src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeIntConfig.h

@@ -0,0 +1,20 @@
+
+// This file is used to separate the changes you make to personalize the 
+// Pin Change Interrupt library from any future changes to the library itself.
+// Ideally it would reside in the folder of the current sketch, but I have not 
+// figured out how such a file can be included from a library.
+// Nothing is required to be defined within this file since default values are
+// defined in the primary PinChangeInt.h file.
+
+// Uncomment the line below to limit the handler to servicing a single interrupt per invocation.
+//#define	DISABLE_PCINT_MULTI_SERVICE
+
+// Define the value MAX_PIN_CHANGE_PINS to limit the number of pins that may be 
+// used for pin change interrupts. This value determines the number of pin change 
+// interrupts supported for all ports.
+//#define	MAX_PIN_CHANGE_PINS 2
+
+// declare ports without pin change interrupts used
+//#define	NO_PORTB_PINCHANGES
+//#define	NO_PORTC_PINCHANGES
+//#define	NO_PORTD_PINCHANGES

src/dcaitirobot/embedded/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

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];
+		float f[4];
-		uint8_t b[8];
+		uint8_t b[16];
 	} u;
-	u.f[0] = value_right;
+	u.f[0] = speed_right;
-	u.f[1] = value_left;
+	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/frames.gv

@@ -0,0 +1,14 @@
+digraph G {
+"base_link" -> "velodyne"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "base_footprint"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"chassis" -> "camera_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "chassis"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_link" -> "camera_link_optical"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"chassis" -> "caster_wheel_back"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"chassis" -> "caster_wheel_front"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"chassis" -> "laser_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 1688907915.1191485"[ shape=plaintext ] ;
+}->"base_link";
+}

src/dcaitirobot/launch/launch_slam.py

@@ -0,0 +1,50 @@
+import os
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+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
+        'config',
+        'slam.yml'
+        )
+    
+    slam_toolbox = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(
+                    get_package_share_directory('slam_toolbox'),'launch','online_async_launch.py'
+                )]), 
+                launch_arguments={
+                    'use_sim_time': 'true', 
+                    'slam_params_file': slam_params}.items()
+    )
+
+    node_fix_lidar = Node(
+                package="dcaitirobot",
+                executable="fixlidar",
+                name="fixlidar",
+            )
+
+    node_scale_odom = Node(
+                package="dcaitirobot",
+                executable="scaleodom",
+                name="scaleodom",
+            )
+
+    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, tfs1, tfs2, node_fix_lidar, node_scale_odom
+    ])

src/dcaitirobot/launch/start_turtle_slam_sim.py

@@ -28,17 +28,17 @@ def generate_launch_description():
                     )
                 )
             ),
-            IncludeLaunchDescription(
+            # IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(
+            #     PythonLaunchDescriptionSource(
-                    str(
+            #         str(
-                        (
+            #             (
-                            Path(get_package_share_directory("slam_toolbox")).absolute()
+            #                 Path(get_package_share_directory("slam_toolbox")).absolute()
-                            / "launch"
+            #                 / "launch"
-                            / "online_async_launch.py"
+            #                 / "online_async_launch.py"
-                        )
+            #             )
-                    )
+            #         )
-                ),
+            #     ),
-                launch_arguments=[("use_sim_time", "True")],
+            #     launch_arguments=[("use_sim_time", "True")],
-            ),
+            # ),
         ]
     )

src/dcaitirobot/setup.py

@@ -12,7 +12,8 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name), glob('launch/*.launch.py'))
+        (os.path.join('share', package_name), glob('launch/*.py')),
+        (os.path.join('share', 'config'), glob('config/*.yml')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -27,6 +28,8 @@ setup(
             'twistcalc = dcaitirobot.twist:main',
             'serial_comms = dcaitirobot.serial_comms:main',
             'joy = dcaitirobot.joy:main',
+            "fixlidar = dcaitirobot.handle_nan_scan:main",
+            "scaleodom = dcaitirobot.scale_odom:main"
         ],
     },
 )

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

@@ -20,12 +20,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)
@@ -48,7 +48,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;
@@ -90,13 +90,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()
     return return_type::ERROR;
   }
 
-  arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel);
+  arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel, l_wheel_.pos, r_wheel_.pos);
-  // 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());
   // convert rpm to rad/s
-  l_wheel_.vel = (l_wheel_.vel / 60) * 2 * M_PI * 0.049 ;
+  l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
-  r_wheel_.vel = (r_wheel_.vel / 60) * 2 * M_PI * 0.049 ;
+  r_wheel_.vel = RPMSTORADS(r_wheel_.vel);
 
     
-  // Force the wheel position
+  // convert rev to rad
-  l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
+  l_wheel_.pos = REVSTORAD(l_wheel_.pos);
-  r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
+  r_wheel_.pos = REVSTORAD(r_wheel_.pos);
   return return_type::OK;  
 }
 
@@ -128,9 +130,7 @@ return_type DiffDriveArduino::write()
     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 );
+  arduino_.setMotorValues(RADSTORPM(l_wheel_.cmd), RADSTORPM(r_wheel_.cmd));
-  std::string msg = "Left Wheel Command: " + std::to_string(l_wheel_.cmd* 30 / M_PI);
-  RCLCPP_INFO(logger_, msg.c_str());
 
   return return_type::OK;  
 }

src/xacro

@@ -0,0 +1 @@
+Subproject commit 344e25b49f3a01a2389f0f30304ed667d68a82bd