add kinetic ver

Geonhee-LEE · Feb 1, 2021 · 1b4f07b · 1b4f07b
1 parent 8451fec
commit 1b4f07b
Show file tree

Hide file tree

Showing 56 changed files with 10,530 additions and 1,181 deletions.
diff --git a/mpc.csv b/mpc.csv
@@ -0,0 +1,24 @@
+idx,car_position_x,car_position_y,cte,etheta,cmd_vel.linear.x,cmd_vel.angular.z,global_path_x,global_path_y
+1,4.06742e-06,-4.53102e-05,0.00926487,0.00140305,0,0,3.51564e-06,-3.96204e-05
+2,7.38142e-05,-0.00114166,0.00928039,0.00141002,0.100001,-0.00792939,3.51564e-06,-3.96204e-05
+3,0.000702181,-0.0109992,0.00941398,0.00094385,0.133659,-0.015593,3.51564e-06,-3.96204e-05
+4,0.00160616,-0.0253887,0.00960671,-0.000371133,0.231998,-0.0217829,3.51564e-06,-3.96204e-05
+5,0.00302588,-0.0484187,0.00990355,-0.00181515,0.265673,-0.0219183,3.51564e-06,-3.96204e-05
+6,0.0048129,-0.0781866,0.010266,-0.00367952,0.364466,-0.0294903,3.51564e-06,-3.96204e-05
+7,0.00698568,-0.115582,0.0106847,-0.00628338,0.432127,-0.0322732,3.51564e-06,-3.96204e-05
+8,0.00943477,-0.159865,0.0111511,-0.00874071,0.496951,-0.0351276,3.51564e-06,-3.96204e-05
+9,0.0121527,-0.211657,0.0116379,-0.0110288,0.571477,-0.0393889,3.51564e-06,-3.96204e-05
+10,0.0150015,-0.269586,0.0121193,-0.0146801,0.664989,-0.0453167,3.51564e-06,-3.96204e-05
+11,0.0180618,-0.33577,0.00942229,0.0112178,0.697224,-0.0431705,0.0152932,-0.232798
+12,0.0211887,-0.408686,0.00865328,0.00729213,0.796029,-0.073792,0.0152932,-0.232798
+13,0.0243632,-0.490929,0.00778472,0.00247313,0.863636,-0.0661858,0.0152932,-0.232798
+14,0.0272258,-0.580088,0.00992778,0.00108066,0.954938,-0.0697956,0.0197671,-0.225691
+15,0.029007,-0.645137,0.00909146,-0.00357259,1.02434,-0.0704471,0.0197671,-0.225691
+16,0.0319021,-0.783877,0.00756248,-0.0126184,1.07108,-0.0653757,0.0197671,-0.225691
+17,0.0332444,-0.892034,0.0236101,-0.0214723,1.13474,-0.0544459,0.0350796,-0.229129
+18,0.0341,-1.00729,0.0230885,-0.0241143,1.16584,0.00325706,0.0350796,-0.229129
+19,0.0346231,-1.0863,0.016218,-0.0269685,1.21161,-0.0158062,0.0267556,-0.223481
+20,0.0354991,-1.24898,0.0150198,-0.0289692,1.23525,-0.0192623,0.0267556,-0.223481
+21,0.0359297,-1.37463,0.0148416,-0.0237969,1.26743,-0.0161159,0.0409796,-1.18519
+22,0.036174,-1.50198,0.0156906,-0.0244223,1.29396,0.0403493,0.0409796,-1.18519
+23,0.0365823,-1.63241,0.015288,-0.0204788,1.31206,0.0227685,0.0407385,-1.16433
diff --git a/mpc_ros/CMakeLists.txt b/mpc_ros/CMakeLists.txt
@@ -15,7 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
   dynamic_reconfigure
   geometry_msgs
   nav_msgs
-  #ackermann_msgs
+  ackermann_msgs
   message_generation
   move_base
   roscpp
@@ -49,20 +49,25 @@ ADD_EXECUTABLE( MPC_Node src/MPC.cpp src/MPC_Node.cpp )
 TARGET_LINK_LIBRARIES(MPC_Node ipopt ${catkin_LIBRARIES} )
 
 # Total navigation with MPC_Node
-ADD_EXECUTABLE( nav_mpc src/navMPC.cpp src/navMPCNode.cpp )
-TARGET_LINK_LIBRARIES(nav_mpc ipopt ${catkin_LIBRARIES} )
+ADD_EXECUTABLE( MPC_Node_last src/MPC_last.cpp src/MPC_lastNode.cpp )
+TARGET_LINK_LIBRARIES(MPC_Node_last ipopt ${catkin_LIBRARIES} )
 
 # Local planner with MPC_Node for tracking
-ADD_EXECUTABLE( tracking_reference_trajectory src/trackRefTraj.cpp src/trackRefTrajNode.cpp )
-TARGET_LINK_LIBRARIES(tracking_reference_trajectory ipopt ${catkin_LIBRARIES} )
+ADD_EXECUTABLE( MPC_local_plan src/MPC_local.cpp src/MPC_local_planNode.cpp )
+TARGET_LINK_LIBRARIES(MPC_local_plan ipopt ${catkin_LIBRARIES} )
+
 
 # Pure Pursuit Node
-add_executable(Pure_Pursuit src/Pure_Pursuit.cpp)
-target_link_libraries(Pure_Pursuit ${catkin_LIBRARIES})
+ADD_EXECUTABLE( Pure_Pursuit_Node src/Pure_Pursuit.cpp)
+TARGET_LINK_LIBRARIES(Pure_Pursuit_Node ${catkin_LIBRARIES})
+
+# Local planner with Pure Pursuit for tracking
+ADD_EXECUTABLE( Pure_Pursuit_localplan src/Pure_Pursuit_local_plan.cpp )
+TARGET_LINK_LIBRARIES(Pure_Pursuit_localplan ${catkin_LIBRARIES})
 
 # Own Global planner for tracking desired trajectory
- add_library(global_planner_lib src/global_planner/global_planner.cpp)
- TARGET_LINK_LIBRARIES(global_planner_lib ipopt ${catkin_LIBRARIES} )
+add_library(global_planner_lib src/global_planner/global_planner.cpp)
+target_link_libraries(global_planner_lib ipopt ${catkin_LIBRARIES} )
 
 #############
 ## Example ##

diff --git a/mpc_ros/include/navMpc.h → mpc_ros/include/MPC_last.h b/mpc_ros/include/navMpc.h → mpc_ros/include/MPC_last.h
diff --git a/mpc_ros/include/trackRefTraj.h → mpc_ros/include/MPC_local.h b/mpc_ros/include/trackRefTraj.h → mpc_ros/include/MPC_local.h
diff --git a/mpc_ros/launch/local_mpc_Gazebo.launch b/mpc_ros/launch/local_mpc_Gazebo.launch
@@ -0,0 +1,74 @@
+<launch>
+
+    <!--  ************** Global Parameters ***************  -->
+    <param name="/use_sim_time" value="true"/>
+    <arg name="controller"  default="pure_pursuit" doc="opt: dwa, mpc, pure_pursuit"/> 
+
+    <!--  ************** GAZEBO Simulator ***************  -->
+    <!--  ************** circle ***************  >
+    <arg name="x_pos" default="0.0"/>
+    <arg name="y_pos" default="-5.0"/>
+    <arg name="z_pos" default="0.0"/>
+    <arg name="roll" default="0"/>
+    <arg name="pitch" default="0"/>
+    <arg name="yaw" default="0"/-->
+    <!--  ************** epitrochoid *************** >
+    <arg name="x_pos" default="3.0"/>
+    <arg name="y_pos" default="0.0"/>
+    <arg name="z_pos" default="0.0"/>
+    <arg name="roll" default="0"/>
+    <arg name="pitch" default="0"/>
+    <arg name="yaw" default="-1.57"/-->
+    <!--  ************** square ***************  -->
+    <arg name="x_pos" default="0.0"/>
+    <arg name="y_pos" default="0.0"/>
+    <arg name="z_pos" default="0.0"/>
+    <arg name="roll" default="0"/>
+    <arg name="pitch" default="0"/>
+    <arg name="yaw" default="1.57075"/>
+    <!--  ************** Infinite ***************  >
+    <arg name="x_pos" default="10.0"/>
+    <arg name="y_pos" default="0.0"/>
+    <arg name="z_pos" default="0.0"/>
+    <arg name="roll" default="0"/>
+    <arg name="pitch" default="0"/>
+    <arg name="yaw" default="1.57075"/-->
+
+    <param name="robot_description" command="$(find xacro)/xacro.py $(find servingbot_description)/urdf/servingbot.urdf.xacro" />
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(find mpc_ros)/worlds/empty.world"/>
+        <arg name="paused" value="false"/>
+        <arg name="use_sim_time" value="true"/>
+        <arg name="gui" value="false"/>
+        <arg name="headless" value="false"/>
+        <arg name="debug" value="false"/>
+    </include>
+
+
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model servingbot -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) -param robot_description" />
+    <include file="$(find servingbot_bringup)/launch/servingbot_remote.launch" />
+
+
+    <!--  ************** MPC Node **************  -->
+    <node name="MPC_local_plan" pkg="mpc_ros" type="MPC_local_plan" output="screen" if="$(eval controller == 'mpc')" >
+        <rosparam file="$(find mpc_ros)/params/mpc_local_square_params.yaml" command="load" />
+        <!--rosparam file="$(find mpc_ros)/params/mpc_local_params.yaml" command="load" /-->
+        <!--rosparam file="$(find mpc_ros)/params/mpc_local_epitrochoid_params.yaml" command="load" /-->
+    </node>
+
+    <!--  ************** Pure pursuit Node **************  -->
+    <node name="Pure_Pursuit_localplan" pkg="mpc_ros" type="Pure_Pursuit_localplan" output="screen" if="$(eval controller == 'pure_pursuit')" >
+        <rosparam file="$(find mpc_ros)/params/pure_pursuit_local_params.yaml" command="load" />
+        <remap from="/pure_pursuit/odom" to="/odom" />
+        <remap from="/pure_pursuit/global_planner" to="/move_base/GlobalPlanner/plan" />
+        <remap from="/pure_pursuit/goal" to="/move_base_simple/goal" />
+        <remap from="/pure_pursuit/cmd_vel" to="/cmd_vel" />
+
+    </node>
+
+
+    <!--  ************** Visualisation **************  -->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_ros)/rviz/rviz_navigation.rviz"/>
+
+</launch>
diff --git a/mpc_ros/launch/mpc.launch b/mpc_ros/launch/mpc.launch
@@ -0,0 +1,89 @@
+<launch>
+
+    <!--  ************** Global Parameters ***************  -->
+
+    <param name="/use_sim_time" value="true"/>
+    <arg name="controller"  default="mpc" doc="opt: dwa, mpc, pure_pursuit"/> 
+    <arg name="map"  default="sq" doc="opt: sq,i, small_s, big_s, xl_s, hitech, empty"/> 
+
+    <arg name="x_pos" default="0.0"/>
+    <arg name="y_pos" default="0.0"/>
+    <arg name="z_pos" default="0.0"/>
+    <arg name="roll" default="0"/>
+    <arg name="pitch" default="0"/>
+    <arg name="yaw" default="0"/>
+    <arg name="yaw_sq" default="-1.5701"/>
+
+    <!--  ************** servingbot ***************  -->
+
+    <include file="$(find servingbot_bringup)/launch/servingbot_remote.launch"/>
+
+    <!--  ************** Map Server **************  -->
+    <node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_ros)/map/sq_world.yaml" output="screen" if="$(eval map == 'sq')">
+        <param name="frame_id" value="/map"/>
+    </node>
+
+    <node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_ros)/map/i_world.yaml" output="screen" if="$(eval map == 'i')">
+        <param name="frame_id" value="/map"/>
+    </node>
+
+    <node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_ros)/map/vi_medium_s.yaml" output="screen" if="$(eval map == 'big_s')">
+        <param name="frame_id" value="/map"/>
+    </node>
+
+    <node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_ros)/map/vi_large_s.yaml" output="screen" if="$(eval map == 'xl_s')">
+        <param name="frame_id" value="/map"/>
+    </node>
+
+    <!--  ************** Localization **************  -->
+
+    <node pkg="amcl" type="amcl" name="amcl" output="screen">
+        <rosparam file="$(find mpc_ros)/params/2amcl_params.yaml" command="load" />
+        <param name="initial_pose_x"            value="$(arg x_pos)"/>
+        <param name="initial_pose_y"            value="$(arg y_pos)"/>
+        <param name="initial_pose_a"            value="$(arg yaw_sq)" if="$(eval map == 'sq')"/>
+        <param name="initial_pose_a"            value="$(arg yaw)" if="$(eval map != 'sq')"/>
+    </node>
+
+    <!--  ************** Navigation ***************  -->
+
+    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+        <rosparam file="$(find mpc_ros)/params/carlike/2costmap_common_params.yaml" command="load" ns="global_costmap" />
+        <rosparam file="$(find mpc_ros)/params/carlike/2costmap_common_params.yaml" command="load" ns="local_costmap" />
+        <rosparam file="$(find mpc_ros)/params/carlike/2local_costmap_params.yaml" command="load" />
+        <rosparam file="$(find mpc_ros)/params/carlike/2global_costmap_params.yaml" command="load" />        
+
+	<!-- Global Planner -->
+        <param name="base_global_planner" value="global_planner/GlobalPlanner" />
+        <param name="planner_frequency" value="1.0" />
+        <param name="planner_patience" value="5.0" />
+        <rosparam file="$(find mpc_ros)/params/global_planner_params.yaml" command="load" />
+
+        <!-- Local Planner -->
+
+        <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
+
+        <!-- Our carlike robot is not able to rotate in place -->
+        <param name="clearing_rotation_allowed" value="false" /> 
+        <!-- external controller -->
+        <remap from="/cmd_vel" to="/fake_cmd" unless="$(eval controller == 'dwa')"/>
+    </node>
+
+    <!--  ************** MPC Node **************  -->
+    <node name="MPC_Node_last" pkg="mpc_ros" type="MPC_Node_last" output="screen" if="$(eval controller == 'mpc')" >
+        <rosparam file="$(find mpc_ros)/params/mpc_last_params.yaml" command="load" />
+    </node>
+
+    <!--  ************** Pure Pursuit **************  -->
+    <node name="Pure_Pursuit_Node" pkg="mpc_ros" type="Pure_Pursuit_Node" output="screen" if="$(eval controller == 'pure_pursuit')" >
+        <rosparam file="$(find mpc_ros)/params/pure_pursuit_params.yaml" command="load" />
+        <remap from="/pure_pursuit/odom" to="/odom" />
+        <remap from="/pure_pursuit/global_planner" to="/move_base/GlobalPlanner/plan" />
+        <remap from="/pure_pursuit/goal" to="/move_base_simple/goal" />
+        <remap from="/pure_pursuit/cmd_vel" to="/cmd_vel" />
+    </node>
+
+    <!--  ************** Visualisation **************  -->
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_ros)/rviz/total_rviz_navigation.rviz"/>
+</launch>
diff --git a/mpc_ros/launch/nav_gazebo.launch → mpc_ros/launch/mpc_gazebo.launch b/mpc_ros/launch/nav_gazebo.launch → mpc_ros/launch/mpc_gazebo.launch
@@ -1,10 +1,8 @@
 <launch>
 
     <!--  ************** Global Parameters ***************  -->
-    <param name="use_sim_time" value="true"/>
+    <param name="/use_sim_time" value="true"/>
     <arg name="controller"  default="mpc" doc="opt: dwa, mpc, pure_pursuit"/> 
-    <arg name="model"  default="serving_bot" doc="opt: serving_bot"/> 
-    <arg name="tf_prefix"  default=""/> 
 
     <!--  ************** GAZEBO Simulator ***************  -->
     <arg name="x_pos" default="0.0"/>
@@ -15,8 +13,7 @@
     <arg name="yaw" default="-1.507"/>
     <arg name="gui" default="true"/>
 
-    <!--  ************** Robot model ***************  -->
-    <param name="robot_description" command="$(find xacro)/xacro.py $(find servingbot_description)/urdf/servingbot.urdf.xacro" if="$(eval model == 'serving_bot')"/>
+    <param name="robot_description" command="$(find xacro)/xacro.py $(find servingbot_description)/urdf/servingbot.urdf.xacro" />
 
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
         <arg name="world_name" value="$(find mpc_ros)/worlds/sq_world.world"/>
@@ -27,65 +24,66 @@
         <arg name="debug" value="false"/>
     </include>
 
+    <!--  ************** servingbot ***************  -->
 
     <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model servingbot -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) -param robot_description" />
-
-    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
-        <param name="publish_frequency" type="double" value="50.0" />
-    </node>
+    <include file="$(find servingbot_bringup)/launch/servingbot_remote.launch" />
 
 
-    <!--  ************** Map **************  -->
+    <!--  ************** Map Server **************  -->
     <node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_ros)/map/sq_world.yaml" output="screen">
-        <param name="frame_id" value="map"/>
+        <param name="frame_id" value="/map"/>
     </node>
 
 
     <!--  ************** Localization **************  -->
 
     <node pkg="amcl" type="amcl" name="amcl" output="screen">
-        <rosparam file="$(find mpc_ros)/params/amcl_params.yaml" command="load" />
+        <rosparam file="$(find mpc_ros)/params/2amcl_params.yaml" command="load" />
         <param name="initial_pose_x"            value="$(arg x_pos)"/>
         <param name="initial_pose_y"            value="$(arg y_pos)"/>
         <param name="initial_pose_a"            value="$(arg yaw)"/>
     </node>
 
-    <!--  ************** Navigation ***************  -->    
+    <!--  ************** Navigation ***************  -->
+
     <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-        <rosparam file="$(find mpc_ros)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
-        <rosparam file="$(find mpc_ros)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
-        <rosparam file="$(find mpc_ros)/params/local_costmap_params.yaml" command="load" />
-        <rosparam file="$(find mpc_ros)/params/global_costmap_params.yaml" command="load" />    
+        <rosparam file="$(find mpc_ros)/params/carlike/2costmap_common_params.yaml" command="load" ns="global_costmap" />
+        <rosparam file="$(find mpc_ros)/params/carlike/2costmap_common_params.yaml" command="load" ns="local_costmap" />
+        <rosparam file="$(find mpc_ros)/params/carlike/2local_costmap_params.yaml" command="load" />
+        <rosparam file="$(find mpc_ros)/params/carlike/global_costmap_params.yaml" command="load" />        
 
-        <!-- Global Planner -->
+<!-- Global Planner -->
         <param name="base_global_planner" value="global_planner/GlobalPlanner" />
         <param name="planner_frequency" value="1.0" />
         <param name="planner_frequency" value="0.0" if="$(eval controller == 'pure_pursuit')"/>
         <param name="planner_patience" value="5.0" />
         <rosparam file="$(find mpc_ros)/params/global_planner_params.yaml" command="load" />
-        
+
         <!-- Local Planner -->
-        <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"  if="$(eval controller == 'mpc')"/>
-        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"  if="$(eval controller == 'dwa')"/>    
-
+        <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
+        <!-- Our carlike robot is not able to rotate in place -->
+        <param name="clearing_rotation_allowed" value="false" /> 
         <!-- external controller -->
         <remap from="/cmd_vel" to="/fake_cmd" unless="$(eval controller == 'dwa')"/>
     </node>
 
     <!--  ************** MPC Node **************  -->
-    <node name="nav_mpc" pkg="mpc_ros" type="nav_mpc" output="screen" if="$(eval controller == 'mpc')" >
+    <node name="MPC_Node_last" pkg="mpc_ros" type="MPC_Node_last" output="screen" if="$(eval controller == 'mpc')" >
         <rosparam file="$(find mpc_ros)/params/mpc_last_params.yaml" command="load" />
     </node>
 
     <!--  ************** Pure Pursuit **************  -->
-    <node name="Pure_Pursuit" pkg="mpc_ros" type="Pure_Pursuit" output="screen" if="$(eval controller == 'pure_pursuit')" >
+    <node name="Pure_Pursuit_Node" pkg="mpc_ros" type="Pure_Pursuit_Node" output="screen" if="$(eval controller == 'pure_pursuit')" >
         <rosparam file="$(find mpc_ros)/params/pure_pursuit_params.yaml" command="load" />
+
         <remap from="/pure_pursuit/odom" to="/odom" />
         <remap from="/pure_pursuit/global_planner" to="/move_base/GlobalPlanner/plan" />
         <remap from="/pure_pursuit/goal" to="/move_base_simple/goal" />
         <remap from="/pure_pursuit/cmd_vel" to="/cmd_vel" />
     </node>
 
     <!--  ************** Visualisation **************  -->
-    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_ros)/params/total_rviz_navigation.rviz"/>
+
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_ros)/rviz/total_rviz_navigation.rviz"/>
 </launch>
diff --git a/mpc_ros/launch/record_odom_traj.launch b/mpc_ros/launch/record_odom_traj.launch