Skip to content
Snippets Groups Projects
Commit 8e8d8282 authored by Akshay Naik's avatar Akshay Naik
Browse files

Added mp2 source code

parent 0a815878
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/mp2/CMakeLists.txt 0 → 100644
+ 207
− 0
View file @ 8e8d8282
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)
src/mp2/launch/mp2.launch 0 → 100644
+ 32
− 0
View file @ 8e8d8282
<?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>
src/mp2/package.xml 0 → 100644
+ 71
− 0
View file @ 8e8d8282
<?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>
src/mp2/src/controller.py 0 → 100644
+ 103
− 0
View file @ 8e8d8282
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)
src/mp2/src/main.py 0 → 100644
+ 74
− 0
View file @ 8e8d8282
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")
src/mp2/src/set_pos.py 0 → 100644
+ 76
− 0
View file @ 8e8d8282
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)
src/mp2/src/util.py 0 → 100644
+ 22
− 0
View file @ 8e8d8282
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
src/mp2/src/waypoint_list.py 0 → 100644
+ 124
− 0
View file @ 8e8d8282
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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment