add kinetic ver

tomson784 · 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