add final project task1

ROS_Core/src/Labs/FinalProject/CMakeLists.txt

@@ -0,0 +1,122 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(final_project)
+
+
+## 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
+  dynamic_reconfigure
+  rospy 
+  std_msgs
+  nav_msgs
+  racecar_msgs
+  racecar_routing
+)
+
+# 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  # Or other packages containing 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()
+
+
+###################################
+## 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(
+    CATKIN_DEPENDS 
+    rospy
+    std_msgs 
+    racecar_msgs 
+    nav_msgs 
+    dynamic_reconfigure
+)
+
+#############
+## 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
+install(PROGRAMS
+  # scripts/task2_world_node.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+

ROS_Core/src/Labs/FinalProject/launch/task1_simulation.launch

@@ -0,0 +1,61 @@
+<launch>
+    <!-- This fix the library linkage issue in RoboStack -->
+    <env name="LD_PRELOAD" value="$(env CONDA_PREFIX)/lib/libyaml-cpp.so" />
+    <arg name="output"              default="screen"                doc="display output to screen or log file"/>
+    <arg name="odom_topic"          default="/Simulation/Pose"      doc="ROS topic for the pose"/>
+    <arg name="control_topic"       default="/Control"              doc="ROS topic for control input"/>
+    <arg name="lane_change_cost"    default="1"                     doc="cost of lane change"/>
+    <arg name="pub_rate"            default="30"                    doc="rate at which to publish the pose"/>
+    <arg name="init_x"              default="3"                     doc="initial x position"/>
+    <arg name="init_y"              default="0.15"                     doc="initial y position"/>
+    <arg name="init_yaw"            default="0"                     doc="initial z position"/>
+    <arg name="num_dyn_obs"         default="0"                     doc="number of dynamic obsects"/>
+    <arg name="num_static_obs"      default="20"                     doc="number of static obsects"/>
+    <arg name="static_obs_size"     default="0.2"                   doc="type of static obsects"/>
+    <arg name="static_obs_topic"    default="/Obstacles/Static"     doc="ROS topic for static obstacles"/>
+    <arg name="dyn_obs_topic"       default="/Obstacles/Dynamic"    doc="ROS topic for dynamic obstacles"/>
+
+    <rosparam file="$(find final_project)/task1.yaml" command="load" />
+
+    <node pkg="racecar_interface" type="simulation_node.py" name="simulation_node" output="$(arg output)">
+        <param name="odom_topic"           value="$(arg odom_topic)" />
+        <param name="control_topic"        value="$(arg control_topic)" />
+        <param name="pub_rate"             value="$(arg pub_rate)" />
+        <param name="init_x"               value="$(arg init_x)" />
+        <param name="init_y"               value="$(arg init_y)" />
+        <param name="init_yaw"             value="$(arg init_yaw)" />
+    </node>
+
+    <node pkg="racecar_interface" type="traffic_simulation_node.py" name="traffic_simulation_node" output="$(arg output)">
+        <param name="map_file"             value="$(find racecar_routing)/cfg/track.pkl" />
+        <param name="num_dyn_obs"          value="$(arg num_dyn_obs)" />
+        <param name="num_static_obs"       value="$(arg num_static_obs)" />
+        <param name="static_obs_size"      value="$(arg static_obs_size)" />
+        <param name="static_obs_topic"     value="$(arg static_obs_topic)" />
+        <param name="dyn_obs_topic"        value="$(arg dyn_obs_topic)" />
+        <param name="pub_rate"             value="$(arg pub_rate)" />
+    </node>
+
+    <node pkg="racecar_interface" type="visualization_node.py" name="visualization_node" output="$(arg output)">
+        <param name="odom_topic"           value="$(arg odom_topic)" />
+        <param name="control_topic"        value="$(arg control_topic)"/>
+    </node>
+
+    <include file="$(find racecar_routing)/launch/visualize_map.launch">
+            <arg name="output"              value="$(arg output)"/>
+    </include>
+
+    <include file="$(find racecar_routing)/launch/routing.launch">
+            <arg name="output"              value="$(arg output)"/>
+            <arg name="odom_topic"          value="$(arg odom_topic)"/>
+            <arg name="lane_change_cost"    value="$(arg lane_change_cost)"/>
+            <arg name="click_goal"          value="false"/>
+    </include>
+
+    <node type="rviz" name="rviz" pkg="rviz" 
+        args="-d $(find racecar_interface)/rviz/simulation.rviz" />
+
+    <node type="rqt_gui" name="rqt_gui" pkg="rqt_gui" 
+        args="--perspective-file $(find racecar_interface)/rviz/rqt_sim.perspective"/>
+
+</launch>

ROS_Core/src/Labs/FinalProject/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>final_project</name>
+  <version>0.1.0</version>
+  <description>The final_project package</description>
+
+  <maintainer email="[email protected]">Zixu Zhang</maintainer>
+  <license>BSD</license>
+
+  <author >Zixu Zhang</author>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>rospy</depend>
+  <depend>racecar_routing</depend>
+  <depend>std_msgs</depend>
+  <depend>racecar_msgs</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>nav_msgs</depend>
+</package>

ROS_Core/src/Labs/FinalProject/setup.py

@@ -0,0 +1,9 @@
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['task2_world'],
+    package_dir={'': 'scripts'},
+)
+
+setup(**d)

ROS_Core/src/Labs/FinalProject/task1.yaml

@@ -0,0 +1,36 @@
+goal_1: [3.15, 0.15]
+goal_2: [3.15, 0.47]
+goal_3: [5.9, 3.5]
+goal_4: [5.6, 3.5]
+goal_5: [0.15, 3.5]
+goal_6: [0.45, 3.5]
+goal_7: [3, 1.1]
+goal_8: [3, 0.8]
+goal_9: [3, 2.2]
+goal_10: [0.75,2.1]
+goal_11: [0.75,4.3]
+goal_12: [4.6,4.6]
+goal_order: [1, 3, 9, 8, 10, 11, 12, 6, 2, 4, 9, 7, 9, 12, 5, 1]
+
+static_obs_location: [
+                  [0.15,4.6], 
+                  [0.55, 5.5],
+                  [0.2, 1.5],
+                  [0.85,0.85],
+                  [1.6, 0.15],
+                  [1.8, 0.15],
+                  [2.5, 0.45],
+                  [4.0, 0.15],
+                  [4.2, 0.15],
+                  [4.4, 0.15],
+                  [5.2, 0.6],
+                  [5.9, 1.3],
+                  [5.6, 2],
+                  [5.9, 2.7],
+                  [5.9, 4.1],
+                  [5.9, 4.3],
+                  [5.6, 5.0],
+                  [5.5, 5.2],
+                  [1.5, 0.83],
+                  [1.16, 1.63]
+                  ]