imu code changed && pure pursuit code changed && odometry code updated

README.md

@@ -1,2 +1,20 @@
 # Simulator
 Related to virtual environment testing
+
+## 변경사항
+
+1. path 생성해주는 코드 변경해서 좀 더 촘촘하게 패쓰를 만들어줌.
+
+2. 교차로 path 수정 
+
+3. plugin package에 있는 c++버전 전부 수정
+
+4. mpc_tracking_ws에 mpc 코드들 추가. 
+
+### 기본
+
+ rosrun mpc_tracking mpc_tracking_node.py
+
+### 주차 신호 받으면 주차 이벤트까지 처리하는 것
+
+rosrun mpc_tracking mpc_tracking_node.py

mpc_tracking_ws/.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

mpc_tracking_ws/src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

mpc_tracking_ws/src/full_depthimage_to_laserscan/CHANGELOG.rst

@@ -0,0 +1,60 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package depthimage_to_laserscan
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.7 (2014-06-16)
+------------------
+* Merge pull request `#11 <https://github.com/ros-perception/depthimage_to_laserscan/issues/11>`_ from ros-perception/hydro-devel
+  Update indigo devel with angle fix
+* Merge pull request `#8 <https://github.com/ros-perception/depthimage_to_laserscan/issues/8>`_ from vliedel/hydro-devel
+  fixed angle_increment
+* fixed angle_increment
+* Merge pull request `#7 <https://github.com/ros-perception/depthimage_to_laserscan/issues/7>`_ from bulwahn/hydro-devel
+  check for CATKIN_ENABLE_TESTING
+* check for CATKIN_ENABLE_TESTING
+* Added ARCHIVE DESTINATION
+* Contributors: Chad Rockey, Lukas Bulwahn, vliedel
+
+1.0.6 (2013-10-31)
+------------------
+* Removed y component from projected beam radius.
+
+1.0.5 (2013-08-21)
+------------------
+* No more Willow Garage email.
+* Merge pull request `#4 <https://github.com/ros-perception/depthimage_to_laserscan/issues/4>`_ from ericperko/groovy-devel
+  Cleaned up CMakeLists a bit
+* Cleaned up CMakeLists a bit
+
+1.0.4 (2013-01-22)
+------------------
+* Update version to 1.0.4
+* Merge pull request `#3 <https://github.com/ros-perception/depthimage_to_laserscan/issues/3>`_ from ahendrix/fix_testdeps
+  Fix testdeps
+* Removed redundant test-depends.
+* Remove old makefile.
+
+1.0.3 (2012-12-23)
+------------------
+* fix exported libraries (fix `#2 <https://github.com/ros-perception/depthimage_to_laserscan/issues/2>`_)
+
+1.0.2 (2012-12-19)
+------------------
+* fix dyn reconf
+* Updated description.
+* Updated readme to link to wiki.
+
+1.0.1 (2012-12-14)
+------------------
+* Update package.xml
+  New release for missing catkin build_depend.
+* Merge pull request `#1 <https://github.com/ros-perception/depthimage_to_laserscan/issues/1>`_ from ablasdel/groovy-devel
+  updated catkin buildtool_depend
+* updated catkin buildtool_depend
+
+1.0.0 (2012-12-05)
+------------------
+* Cleanup of the CMakeLists.txt
+* First attempt at Catkinizing.
+* Pushing fuerte-devel to github.
+* Initial commit

mpc_tracking_ws/src/full_depthimage_to_laserscan/CMakeLists.txt

@@ -0,0 +1,56 @@
+# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
+cmake_minimum_required(VERSION 2.8)
+project(full_depthimage_to_laserscan)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED dynamic_reconfigure image_geometry image_transport nodelet roscpp sensor_msgs)
+#find_package(OpenCV REQUIRED)
+
+# Dynamic reconfigure support
+generate_dynamic_reconfigure_options(cfg/Depth.cfg)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES FullDepthImageToLaserScan FullDepthImageToLaserScanROS FullDepthImageToLaserScanNodelet
+  CATKIN_DEPENDS dynamic_reconfigure image_geometry image_transport nodelet roscpp sensor_msgs
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+add_library(FullDepthImageToLaserScan src/DepthImageToLaserScan.cpp)
+target_link_libraries(FullDepthImageToLaserScan ${catkin_LIBRARIES})
+target_compile_options(FullDepthImageToLaserScan PRIVATE -Wall -fopt-info-vec-optimized -ftree-vectorize  -fno-math-errno -funsafe-math-optimizations)
+target_compile_options(FullDepthImageToLaserScan PUBLIC -std=c++11)
+
+
+add_library(FullDepthImageToLaserScanROS src/DepthImageToLaserScanROS.cpp)
+add_dependencies(FullDepthImageToLaserScanROS ${PROJECT_NAME}_gencfg)
+target_link_libraries(FullDepthImageToLaserScanROS FullDepthImageToLaserScan ${catkin_LIBRARIES})
+
+add_library(FullDepthImageToLaserScanNodelet src/DepthImageToLaserScanNodelet.cpp)
+target_link_libraries(FullDepthImageToLaserScanNodelet FullDepthImageToLaserScanROS ${catkin_LIBRARIES})
+
+add_executable(full_depthimage_to_laserscan src/depthimage_to_laserscan.cpp)
+target_link_libraries(full_depthimage_to_laserscan FullDepthImageToLaserScanROS ${catkin_LIBRARIES})
+
+# if(CATKIN_ENABLE_TESTING)
+#   # Test the library
+#   catkin_add_gtest(libtest test/DepthImageToLaserScanTest.cpp)
+#   target_link_libraries(libtest DepthImageToLaserScan ${catkin_LIBRARIES})
+# endif()
+# 
+# # add the test executable, keep it from being built by "make all"
+# add_executable(test_dtl EXCLUDE_FROM_ALL test/depthimage_to_laserscan_rostest.cpp)
+
+# Install targets
+install(TARGETS FullDepthImageToLaserScan FullDepthImageToLaserScanROS FullDepthImageToLaserScanNodelet full_depthimage_to_laserscan
+	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+install(FILES nodelets.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

mpc_tracking_ws/src/full_depthimage_to_laserscan/README.md

@@ -0,0 +1,28 @@
+depthimage_to_laserscan
+=======================
+
+Converts a depth image to a laser scan for use with navigation and localization.
+
+ROS Wiki Page:
+http://www.ros.org/wiki/depthimage_to_laserscan
+
+The package is renamed to allow it to exist side-by-side with the original depthimage_to_laserscan package without any conflicts. A sample launch file for the turtlebot is included.
+
+*Note:*
+Extra revision is added for noetic version of ROS than that from the forked repo.
+
+### Parameters and Getting it Working
+
+You will need to change the `depth_image` arg to match yours (the camera info topic is determined automatically based on the depth image's topic). The `scan` arg specifies the topic that the generated laserscan will be published on. The nodelet expects a rectified depth image, though the raw image can work as long as the camera's distortion is minimal; YMMV. Some deeth image collision avoidance or navigation strategies use decimated depth maps for faster processing. Here, it doesn't make much difference since computation time scales sublinearly with the number of pixels.
+
+`scan_height`: param determines how much of the images is used when generating the laserscan; it can be set  to anything from 1 to (image_height-1). There's no compelling reason to deviate from using the largest value. <BR>
+`output_frame_id`: set to match the frame_id of your depth camera- not the camera's optical frame_id! <BR>
+`range_min`: ignore anything closer than this value; set it to the nearest effective range of your camera. <BR>
+`floor_dist`: set it to the vertical distance of the depth camera above the floor, this allows the floor to be ignored when generating the laserscan. 
+Some trial and error will be needed to find the best value for your use, as the filtering assumes that the camera stays perfectly level with the groundplane, meaning that if the robot pitches forward (such as when slowing rapidly) part of the floor may be falsely registered as an obstacle. A few cm extra is usually enough. <BR>
+`overhead_dist`: the vertical distance from the camera to the highest point on the robot. It serves a similar purpose to `floor_dist`, except that it filters out obstacles that are too high to collide with the robot.
+
+Note that all of the parameters can be dynamically reconfigured, so it shouldn't take too long to find good values for them.
+Just like the original implementation, the nodelet only performs the computations if something subscribes to it, so you can leave it running all the time without negligible cost.
+
+The nodelet publishes the `mask` used to filter points on the topic `mask_image`.  You can visualize this as a pointcloud using [point cloud visualization](http://wiki.ros.org/depth_image_proc#depth_image_proc.2Fpoint_cloud_xyz) by remapping `camera_info` to your depth camera's camera info topic and remapping `image_rect` to `mask_image` (or whatever you choose to remap it to). It visualizes the upper and lower bounds in rviz relative to the robot. As a nodelet, it has negligible cost when nothing subscribes to the generated pointcloud.

mpc_tracking_ws/src/full_depthimage_to_laserscan/cfg/Depth.cfg

@@ -0,0 +1,18 @@
+#! /usr/bin/env python
+
+PACKAGE='full_depthimage_to_laserscan'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+from math import pi
+
+gen = ParameterGenerator()
+#       Name                    Type      Reconfiguration level             Description                                                            Default    Min   Max
+gen.add("scan_height",          int_t,    0,                                "Height of the laser band (in pixels).",                            1,      1,   500)
+gen.add("scan_time",            double_t, 0,                                "Time for the entire scan sweep.",                                  0.033,  0.0, 1.0)
+gen.add("range_min",            double_t, 0,                                "Minimum reported range (in meters).",                              0.45,   0.0, 10.0)
+gen.add("range_max",            double_t, 0,                                "Maximum reported range (in meters).",                              10.0,   0.0, 10.0)
+gen.add("output_frame_id",      str_t,    0,                                "Output frame_id for the laserscan.",   "camera_depth_frame")
+gen.add("floor_dist",           double_t, 0,                                "Vertical distance between camera and floor",                       .25,    0,    1.0)
+gen.add("overhead_dist",           double_t, 0,                                "Vertical distance between camera and top of robot",                       .15,    0,    1.0)
+exit(gen.generate(PACKAGE, "full_depthimage_to_laserscan", "Depth"))