rosparam

config/.vscode/c_cpp_properties.json

@@ -0,0 +1,46 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/thatblueboy/catkin_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/thatblueboy/catkin_ws/src/arm_rdf/include/**",
+        "/home/thatblueboy/catkin_ws/src/cam_cv/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_planners/chomp/chomp_motion_planner/include/**",
+        "/home/thatblueboy/catkin_ws/src/geometric_shapes/include/**",
+        "/home/thatblueboy/catkin_ws/src/maze/include/**",
+        "/home/thatblueboy/maze_ws/src/maze_solver/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_planners/chomp/chomp_interface/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit_resources/prbt_ikfast_manipulator_plugin/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_ros/benchmarks/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_plugins/moveit_ros_control_interface/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_ros/move_group/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_ros/occupancy_map_monitor/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_ros/robot_interaction/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_ros/moveit_servo/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_setup_assistant/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_plugins/moveit_simple_controller_manager/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit_visual_tools/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_planners/pilz_industrial_motion_planner/include/**",
+        "/home/thatblueboy/catkin_ws/src/moveit/moveit_planners/pilz_industrial_motion_planner_testutils/include/**",
+        "/home/thatblueboy/catkin_ws/src/robotiq/robotiq_gazebo/include/**",
+        "/home/thatblueboy/catkin_ws/src/rviz_visual_tools/include/**",
+        "/home/thatblueboy/catkin_ws/src/srdfdom/include/**",
+        "/home/thatblueboy/catkin_ws/src/turtlebot3_simulations/turtlebot3_fake/include/**",
+        "/home/thatblueboy/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/include/**",
+        "/home/thatblueboy/catkin_ws/src/turtlebot3/turtlebot3_slam/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

config/.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/thatblueboy/catkin_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

config/params.yaml

@@ -0,0 +1,10 @@
+vKp: 0.05
+vKi: 0.00
+vKd: 0.00
+vdt: 0.0333
+vmax: 0.2
+omegaKp: 0.1
+omegaKi: 0.00
+omegaKd: 0.00
+omegadt: 0.0333
+omegaMax: 2.8

launch/launch_only_turtlebot.launch

@@ -6,14 +6,18 @@
         <arg name = "z_pos" default = "0"/>
         <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
 
-
-        <!-- Spawn the world -->
+        <include file = "$(find maze_solver)/launch/load_rosparam.launch">
+        </include>
+
         <include file="$(find gazebo_ros)/launch/empty_world.launch">
     	</include>        
 
-
-
-        <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
+        <node 
+        pkg="gazebo_ros" 
+        type="spawn_model" 
+        name="spawn_urdf" 
+        args="-urdf -model turtlebot3_$(arg model) 
+        -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
 
 
 </launch>

launch/load_rosparam.launch

@@ -0,0 +1,3 @@
+<launch>
+<rosparam command = "load" file = "$(find maze_solver)/config/params.yaml" />
+</launch>

scripts/PID.py

@@ -2,11 +2,11 @@
 import math
 
 import rospy
+import rosparam
 from geometry_msgs.msg import Twist
 from nav_msgs.msg import Odometry
 from tf.transformations import euler_from_quaternion
 
-
 class PID():
     '''
     single variable PID controller
@@ -47,9 +47,10 @@ def findVout(self, err:float):
         else:
             return self.vmax
 
-class main():
-    def __init__(self):
-        self.points = [(0, 5), (6, 2)]
+class followPath():
+
+    def __init__(self, path):
+        self.points = path
         self.numOfNodes = len(self.points)
         self.x = 0
         self.y = 0
@@ -58,16 +59,17 @@ def __init__(self):
         self.linearPIDflag = False
         self.velocity = Twist()
 
-        vKp = 0.1
-        vKi = 0.00
-        vKd = 0.00
-        vdt = 0.0333
-        vmax = 0.2
-        omegaKp = 0.1
-        omegaKi = 0.00
-        omegaKd = 0.00
-        omegadt = 0.0333
-        omegaMax = 2.8
+        vKp = rospy.get_param("vKp")
+        vKi = rospy.get_param("vKi")
+        vKd = rospy.get_param("vKd")
+        vdt = rospy.get_param("vdt")
+        vmax = rospy.get_param("vmax")
+        omegaKp = rospy.get_param("omegaKp")
+        omegaKi = rospy.get_param("omegaKi")
+        omegaKd = rospy.get_param("omegaKd")
+        omegadt = rospy.get_param("omegadt")
+        omegaMax = rospy.get_param("omegaMax")
+
         self.vPID = PID(vKp, vKi, vKd, vmax, vdt)
         self.omegaPID = PID(omegaKp, omegaKi, omegaKd, omegaMax, omegadt)
 
@@ -132,4 +134,5 @@ def reached(self, x, y):
         return False
 
 if __name__ == '__main__':
-    main()
+    listOfPath = [(0, 5), (6, 2)]
+    followPath(listOfPath)