From 8e8d82824f0e3f94a5c1c26f3e6db006a7d59c48 Mon Sep 17 00:00:00 2001
From: Akshay Naik <akshaynaik@Akshays-MacBook-Air.local>
Date: Thu, 15 Feb 2024 22:34:45 -0600
Subject: [PATCH] Added mp2 source code
---
src/mp2/CMakeLists.txt | 207 +++++++++++++++++++++++++++++++++++
src/mp2/launch/mp2.launch | 32 ++++++
src/mp2/package.xml | 71 ++++++++++++
src/mp2/src/controller.py | 103 +++++++++++++++++
src/mp2/src/main.py | 74 +++++++++++++
src/mp2/src/set_pos.py | 76 +++++++++++++
src/mp2/src/util.py | 22 ++++
src/mp2/src/waypoint_list.py | 124 +++++++++++++++++++++
8 files changed, 709 insertions(+)
create mode 100644 src/mp2/CMakeLists.txt
create mode 100644 src/mp2/launch/mp2.launch
create mode 100644 src/mp2/package.xml
create mode 100644 src/mp2/src/controller.py
create mode 100644 src/mp2/src/main.py
create mode 100644 src/mp2/src/set_pos.py
create mode 100644 src/mp2/src/util.py
create mode 100644 src/mp2/src/waypoint_list.py
diff --git a/src/mp2/CMakeLists.txt b/src/mp2/CMakeLists.txt
new file mode 100644
index 0000000..9fa6226
--- /dev/null
+++ b/src/mp2/CMakeLists.txt
@@ -0,0 +1,207 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(mp2)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ gazebo_ros
+ roscpp
+ rospy
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES mp2
+# CATKIN_DEPENDS gazebo_ros roscp rospy std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/mp2.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/mp2_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_mp2.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/src/mp2/launch/mp2.launch b/src/mp2/launch/mp2.launch
new file mode 100644
index 0000000..af693b3
--- /dev/null
+++ b/src/mp2/launch/mp2.launch
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+
+<launch>
+ <arg name="paused" default="false"/>
+ <arg name="use_sim_time" default="true"/>
+ <arg name="gui" default="true"/>
+ <arg name="headless" default="false"/>
+ <arg name="debug" default="false"/>
+
+ <include file="$(find gazebo_ros)/launch/empty_world.launch">
+ <arg name="world_name" value="$(find gem_gazebo)/worlds/smaller_track_with_starting_point.world"/>
+ <arg name="debug" value="$(arg debug)" />
+ <arg name="gui" value="$(arg gui)" />
+ <arg name="paused" value="$(arg paused)"/>
+ <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+ <arg name="headless" value="$(arg headless)"/>
+ <arg name="verbose" value="false"/>
+ </include>
+
+
+ <!-- Spawn the GEM CAR -->
+ <include file="$(find gem_gazebo)/launch/gem_vehicle.launch">
+ <!-- <arg name="namespace" value="/gem"/> -->
+ <arg name="x" value="0.0"/>
+ <arg name="y" value="-98"/>
+ <arg name="z" value="1.0"/>
+ </include>
+
+ <!-- RViz -->
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find gem_description)/config_rviz/gem_velodyne.rviz" />
+
+</launch>
diff --git a/src/mp2/package.xml b/src/mp2/package.xml
new file mode 100644
index 0000000..918e9a5
--- /dev/null
+++ b/src/mp2/package.xml
@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>mp2</name>
+ <version>0.0.0</version>
+ <description>The mp2 package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="pricejiang@todo.todo">pricejiang</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/mp2</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>gazebo_ros</build_depend>
+ <build_depend>roscp</build_depend>
+ <build_depend>rospy</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_export_depend>gazebo_ros</build_export_depend>
+ <build_export_depend>roscp</build_export_depend>
+ <build_export_depend>rospy</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
+ <exec_depend>gazebo_ros</exec_depend>
+ <exec_depend>roscp</exec_depend>
+ <exec_depend>rospy</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/src/mp2/src/controller.py b/src/mp2/src/controller.py
new file mode 100644
index 0000000..e959a48
--- /dev/null
+++ b/src/mp2/src/controller.py
@@ -0,0 +1,103 @@
+import rospy
+from gazebo_msgs.srv import GetModelState, GetModelStateResponse
+from gazebo_msgs.msg import ModelState
+from ackermann_msgs.msg import AckermannDrive
+import numpy as np
+from std_msgs.msg import Float32MultiArray
+import math
+from util import euler_to_quaternion, quaternion_to_euler
+import time
+
+class vehicleController():
+
+ def __init__(self):
+ # Publisher to publish the control input to the vehicle model
+ self.controlPub = rospy.Publisher("/ackermann_cmd", AckermannDrive, queue_size = 1)
+ self.prev_vel = 0
+ self.L = 1.75 # Wheelbase, can be get from gem_control.py
+ self.log_acceleration = False
+
+ def getModelState(self):
+ # Get the current state of the vehicle
+ # Input: None
+ # Output: ModelState, the state of the vehicle, contain the
+ # position, orientation, linear velocity, angular velocity
+ # of the vehicle
+ rospy.wait_for_service('/gazebo/get_model_state')
+ try:
+ serviceResponse = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
+ resp = serviceResponse(model_name='gem')
+ except rospy.ServiceException as exc:
+ rospy.loginfo("Service did not process request: "+str(exc))
+ resp = GetModelStateResponse()
+ resp.success = False
+ return resp
+
+
+ # Tasks 1: Read the documentation https://docs.ros.org/en/fuerte/api/gazebo/html/msg/ModelState.html
+ # and extract yaw, velocity, vehicle_position_x, vehicle_position_y
+ # Hint: you may use the the helper function(quaternion_to_euler()) we provide to convert from quaternion to euler
+ def extract_vehicle_info(self, currentPose):
+
+ ####################### TODO: Your TASK 1 code starts Here #######################
+ pos_x, pos_y, vel, yaw = 0, 0, 0, 0
+
+ ####################### TODO: Your Task 1 code ends Here #######################
+
+ return pos_x, pos_y, vel, yaw # note that yaw is in radian
+
+ # Task 2: Longtitudal Controller
+ # Based on all unreached waypoints, and your current vehicle state, decide your velocity
+ def longititudal_controller(self, curr_x, curr_y, curr_vel, curr_yaw, future_unreached_waypoints):
+
+ ####################### TODO: Your TASK 2 code starts Here #######################
+ target_velocity = 10
+
+
+ ####################### TODO: Your TASK 2 code ends Here #######################
+ return target_velocity
+
+
+ # Task 3: Lateral Controller (Pure Pursuit)
+ def pure_pursuit_lateral_controller(self, curr_x, curr_y, curr_yaw, target_point, future_unreached_waypoints):
+
+ ####################### TODO: Your TASK 3 code starts Here #######################
+ target_steering = 0
+
+ ####################### TODO: Your TASK 3 code starts Here #######################
+ return target_steering
+
+
+ def execute(self, currentPose, target_point, future_unreached_waypoints):
+ # Compute the control input to the vehicle according to the
+ # current and reference pose of the vehicle
+ # Input:
+ # currentPose: ModelState, the current state of the vehicle
+ # target_point: [target_x, target_y]
+ # future_unreached_waypoints: a list of future waypoints[[target_x, target_y]]
+ # Output: None
+
+ curr_x, curr_y, curr_vel, curr_yaw = self.extract_vehicle_info(currentPose)
+
+ # Acceleration Profile
+ if self.log_acceleration:
+ acceleration = (curr_vel- self.prev_vel) * 100 # Since we are running in 100Hz
+
+
+
+ target_velocity = self.longititudal_controller(curr_x, curr_y, curr_vel, curr_yaw, future_unreached_waypoints)
+ target_steering = self.pure_pursuit_lateral_controller(curr_x, curr_y, curr_yaw, target_point, future_unreached_waypoints)
+
+
+ #Pack computed velocity and steering angle into Ackermann command
+ newAckermannCmd = AckermannDrive()
+ newAckermannCmd.speed = target_velocity
+ newAckermannCmd.steering_angle = target_steering
+
+ # Publish the computed control input to vehicle model
+ self.controlPub.publish(newAckermannCmd)
+
+ def stop(self):
+ newAckermannCmd = AckermannDrive()
+ newAckermannCmd.speed = 0
+ self.controlPub.publish(newAckermannCmd)
diff --git a/src/mp2/src/main.py b/src/mp2/src/main.py
new file mode 100644
index 0000000..45acc54
--- /dev/null
+++ b/src/mp2/src/main.py
@@ -0,0 +1,74 @@
+import rospy
+import numpy as np
+import argparse
+
+from gazebo_msgs.msg import ModelState
+from controller import vehicleController
+import time
+from waypoint_list import WayPoints
+from util import euler_to_quaternion, quaternion_to_euler
+
+def run_model():
+ rospy.init_node("model_dynamics")
+ controller = vehicleController()
+
+ waypoints = WayPoints()
+ pos_list = waypoints.getWayPoints()
+ pos_idx = 1
+
+ target_x, target_y = pos_list[pos_idx]
+
+ def shutdown():
+ """Stop the car when this ROS node shuts down"""
+ controller.stop()
+ rospy.loginfo("Stop the car")
+
+ rospy.on_shutdown(shutdown)
+
+ rate = rospy.Rate(100) # 100 Hz
+ rospy.sleep(0.0)
+ start_time = rospy.Time.now()
+ prev_wp_time = start_time
+
+ while not rospy.is_shutdown():
+ rate.sleep() # Wait a while before trying to get a new state
+
+ # Get the current position and orientation of the vehicle
+ currState = controller.getModelState()
+
+ if not currState.success:
+ continue
+
+ # Compute relative position between vehicle and waypoints
+ distToTargetX = abs(target_x - currState.pose.position.x)
+ distToTargetY = abs(target_y - currState.pose.position.y)
+
+ cur_time = rospy.Time.now()
+ if (cur_time - prev_wp_time).to_sec() > 4:
+ print(f"failure to reach {pos_idx}-th waypoint in time")
+ return False, pos_idx, (cur_time - start_time).to_sec()
+
+ if (distToTargetX < 2 and distToTargetY < 2):
+ # If the vehicle is close to the waypoint, move to the next waypoint
+ prev_pos_idx = pos_idx
+ pos_idx = pos_idx+1
+
+ if pos_idx == len(pos_list): #Reached all the waypoints
+ print("Reached all the waypoints")
+ total_time = (cur_time - start_time).to_sec()
+ print(total_time)
+ return True, pos_idx, total_time
+
+ target_x, target_y = pos_list[pos_idx]
+
+ time_taken = (cur_time- prev_wp_time).to_sec()
+ prev_wp_time = cur_time
+ print(f"Time Taken: {round(time_taken, 2)}", "reached",pos_list[prev_pos_idx][0],pos_list[prev_pos_idx][1],"next",pos_list[pos_idx][0],pos_list[pos_idx][1])
+
+ controller.execute(currState, [target_x, target_y], pos_list[pos_idx:])
+
+if __name__ == "__main__":
+ try:
+ status, num_waypoints, time_taken = run_model()
+ except rospy.exceptions.ROSInterruptException:
+ rospy.loginfo("Shutting down")
diff --git a/src/mp2/src/set_pos.py b/src/mp2/src/set_pos.py
new file mode 100644
index 0000000..f7806ce
--- /dev/null
+++ b/src/mp2/src/set_pos.py
@@ -0,0 +1,76 @@
+import sys
+import os
+import argparse
+
+import numpy as np
+
+import rospy
+from gazebo_msgs.msg import ModelState
+from gazebo_msgs.srv import SetModelState
+from gazebo_msgs.srv import GetModelState
+
+from util import euler_to_quaternion
+
+def getModelState():
+ rospy.wait_for_service('/gazebo/get_model_state')
+ try:
+ serviceResponse = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
+ modelState = serviceResponse(model_name='gem')
+ except rospy.ServiceException as exc:
+ rospy.loginfo("Service did not process request: "+str(exc))
+ return modelState
+
+def setModelState(model_state):
+ rospy.wait_for_service('/gazebo/set_model_state')
+ try:
+ set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
+ resp = set_state(model_state)
+ except rospy.ServiceException as e:
+ rospy.loginfo("Service did not process request: "+str(e))
+
+def set_position(x = 0,y = 0, yaw=0):
+
+ rospy.init_node("set_pos")
+
+ curr_state = getModelState()
+ new_state = ModelState()
+ new_state.model_name = 'gem'
+ new_state.twist.linear.x = 0
+ new_state.twist.linear.y = 0
+ new_state.twist.linear.z = 0
+ new_state.pose.position.x = x
+ new_state.pose.position.y = y
+ new_state.pose.position.z = 1
+ q = euler_to_quaternion([0,0,yaw])
+ new_state.pose.orientation.x = q[0]
+ new_state.pose.orientation.y = q[1]
+ new_state.pose.orientation.z = q[2]
+ new_state.pose.orientation.w = q[3]
+ new_state.twist.angular.x = 0
+ new_state.twist.angular.y = 0
+ new_state.twist.angular.z = 0
+ setModelState(new_state)
+
+if __name__ == "__main__":
+ parser = argparse.ArgumentParser(description = 'Set the x, y position of the vehicle')
+
+ x_default = 0
+ y_default = -98
+ yaw_default = 0
+
+ # x_default = 160
+ # y_default = 96.5
+ # yaw_default = 3.14
+
+
+ parser.add_argument('--x', type = float, help = 'x position of the vehicle.', default = x_default)
+ parser.add_argument('--y', type = float, help = 'y position of the vehicle.', default = y_default)
+ parser.add_argument('--yaw', type = float, help = 'yaw of the vehicle.', default = yaw_default)
+
+ argv = parser.parse_args()
+
+ x = argv.x
+ y = argv.y
+ yaw = argv.yaw
+
+ set_position(x = x, y = y, yaw = yaw)
diff --git a/src/mp2/src/util.py b/src/mp2/src/util.py
new file mode 100644
index 0000000..fcf54a9
--- /dev/null
+++ b/src/mp2/src/util.py
@@ -0,0 +1,22 @@
+import numpy as np
+
+def euler_to_quaternion(r):
+ (roll, pitch, yaw) = (r[0], r[1], r[2])
+ qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
+ qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
+ qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
+ qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
+ return [qx, qy, qz, qw]
+
+def quaternion_to_euler(x, y, z, w):
+ t0 = +2.0 * (w * x + y * z)
+ t1 = +1.0 - 2.0 * (x * x + y * y)
+ roll = np.arctan2(t0, t1)
+ t2 = +2.0 * (w * y - z * x)
+ t2 = +1.0 if t2 > +1.0 else t2
+ t2 = -1.0 if t2 < -1.0 else t2
+ pitch = np.arcsin(t2)
+ t3 = +2.0 * (w * z + x * y)
+ t4 = +1.0 - 2.0 * (y * y + z * z)
+ yaw = np.arctan2(t3, t4)
+ return [roll, pitch, yaw]
\ No newline at end of file
diff --git a/src/mp2/src/waypoint_list.py b/src/mp2/src/waypoint_list.py
new file mode 100644
index 0000000..931919e
--- /dev/null
+++ b/src/mp2/src/waypoint_list.py
@@ -0,0 +1,124 @@
+import numpy as np
+
+
+class WayPoints():
+ def __init__(self):
+ self.pos_list = [
+ [0,-98], # 0th wp
+ [10,-98],
+ [20,-98],
+ [30,-98],
+ [40,-98],
+ [50,-98],
+ [60,-98],
+ [70,-98],
+ [80,-98],
+ [90,-98],
+ [100,-98], # 10th
+ [110,-98],
+ [120,-98],
+ [130,-98],
+ [140,-98],
+ [150,-98],
+ [160,-98],
+ [170,-98],
+ [180,-98],
+ [190,-98],
+ [200,-98], # 20th
+ [210,-98],
+ [220,-97],
+ [230,-95],
+ [240,-92],
+ [250,-87],
+ [260,-81],
+ [270,-74],
+ [280,-63],
+ [290,-49],
+ [295,-37], # 30 th
+ [300,-22],
+ [303,-5],
+ [303,15],
+ [303,25],
+ [303,35],
+ [303,50],
+ [301,63],
+ [295,77],
+ [290,83],
+ [285,87], # 40th
+ [280,90],
+ [275,93],
+ [270,94.5],
+ [250,96.5],
+ [240,96.5],
+ [230,96.5],
+ [220,96.5],
+ [210,96.5],
+ [200,96.5],
+ [190,96.5], # 50th
+ [180,96.5],
+ [170,96.5],
+ [160,96.5],
+ [150,96.5],
+ [140,96.5],
+ [130,96.5],
+ [120,96.5],
+ [110,96.5],
+ [100,96.5],
+ [90,96.5], # 60th
+ [80,96.5],
+ [70,96.5],
+ [60,96.5],
+ [50,96.5],
+ [40,99],
+ [30,103],
+ [20,109],
+ [15,115],
+ [9,125],
+ [5,135], # 70th
+ [4,145],
+ [3,157],
+ [-0.5,170],
+ [-7,180],
+ [-14,186.5],
+ [-29,194],
+ [-45,197],
+ [-56.5,194.5],
+ [-65.5,191.5],
+ [-81.5,177.5], # 80th
+ [-87,169.5],
+ [-90,160],
+ [-92,152],
+ [-97.5,123.5],
+ [-104.5,113],
+ [-118,103],
+ [-132,98.5],
+ [-144,96.5],
+ [-155,95],
+ [-167,91], # 90th
+ [-180.5,79],
+ [-187,68.5],
+ [-191,53],
+ [-191,33],
+ [-191,13],
+ [-191,-3],
+ [-189,-20],
+ [-184.5,-34.5],
+ [-178.5,-50.5],
+ [-166.5,-66], # 100th
+ [-150,-80.5],
+ [-138.5,-87.5],
+ [-125,-93.5],
+ [-113.5,-96.5],
+ [-96,-98],
+ [-86,-98],
+ [-76,-98],
+ [-66,-98],
+ [-56,-98],
+ [-46,-98],
+ [-36,-98],
+ [-26,-98],
+ [-16,-98], # 113-th wp
+ ]
+
+ def getWayPoints(self):
+ return self.pos_list
\ No newline at end of file
--
GitLab