Skip to content

add holonomic node #20

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: melodic
Choose a base branch
from
Open

add holonomic node #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9421905
add holonomic node
CzJaewan Feb 16, 2021
c0d383f
2021_3_5
CzJaewan Mar 5, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions mpc_ros/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,17 @@ link_directories(/usr/lib)
ADD_EXECUTABLE( MPC_Node src/MPC.cpp src/MPC_Node.cpp )
TARGET_LINK_LIBRARIES(MPC_Node ipopt ${catkin_LIBRARIES} )


# General holonomic MPC_Node
ADD_EXECUTABLE( MPC_Node_holo src/MPC_holo.cpp src/MPC_Node_holo.cpp )
TARGET_LINK_LIBRARIES(MPC_Node_holo ipopt ${catkin_LIBRARIES} )


# Simple holonomic MPC_Node
ADD_EXECUTABLE( MPC_Node_holo_simple src/MPC_holo_simple.cpp src/MPC_Node_holo_simple.cpp )
TARGET_LINK_LIBRARIES(MPC_Node_holo_simple 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} )
Expand Down
48 changes: 48 additions & 0 deletions mpc_ros/include/MPC_holo.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
/*
# Copyright 2018 HyphaROS Workshop.
# Developer: HaoChih, LIN ([email protected])
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
*/

#ifndef MPC_HOLO_H
#define MPC_HOLO_H

#include <vector>
#include <map>
#include <Eigen/Core>

using namespace std;

class MPC
{
public:
MPC();

// Solve the model given an initial state and polynomial coefficients.
// Return the first actuatotions.
vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
vector<double> mpc_x;
vector<double> mpc_y;

void LoadParams(const std::map<string, double> &params);

private:
// Parameters for mpc solver
double _max_angvel, _max_throttle, _bound_value;
int _mpc_steps, _x_start, _y_start, _theta_start, _vx_start, _vy_start, _cte_start, _etheta_start, _angvel_start, _ax_start, _ay_start;
std::map<string, double> _params;

};

#endif /* MPC_HOLO_H */
71 changes: 71 additions & 0 deletions mpc_ros/launch/holonomic_nav_gazebo.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,71 @@
<launch>

<!-- ************** Global Parameters *************** -->
<param name="use_sim_time" value="true"/>
<arg name="controller" default="mpc" doc="opt: dwa, mpc, pure_pursuit"/>
<arg name="model" default="kugle" doc="opt: serving_bot, kugle"/>
<arg name="tf_prefix" default=""/>


<!-- Default starting position of robot -->
<arg name="x_pos" default="0.0"/>
<arg name="y_pos" default="0.0"/>
<arg name="z_pos" default="0.2"/>
<arg name="roll" default="0"/>
<arg name="pitch" default="0"/>
<arg name="yaw" default="0"/>

<!-- ************** Map ************** -->
<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"/>
</node>


<!-- ************** Localization ************** -->

<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find mpc_ros)/params/amcl_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 *************** -->
<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" />

<!-- 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')"/>

<!-- 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')" >
<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')" >
<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"/>
</launch>
98 changes: 98 additions & 0 deletions mpc_ros/launch/mpc_local_planner_ackermann.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
<launch>

<!-- ************** Global Parameters *************** -->
<param name="use_sim_time" value="true"/>
<arg name="controller" default="mpc" doc="opt: dwa, mpc, pure_pursuit"/>
<arg name="model" default="holo_bot" doc="opt: serving_bot, holo_bot"/>
<arg name="tf_prefix" default=""/>


<arg name="cmd_timeout" default="0.5"/>


<!-- ************** GAZEBO Simulator *************** -->
<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.507"/>
<arg name="gui" default="false"/>


<!-- ************** Robot model *************** -->
<param name="robot_description"
command="$(find xacro)/xacro $(find mpc_model_description)/urdf/car_like/ackermann_bot.urdf.xacro"/>

<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find mpc_ros)/worlds/sq_world.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>

<!-- Spawn the vehicle. -->
<node name="spawn_vehicle" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model ackermann_bot
-x $(arg x_pos) -y $(arg x_pos) -z $(arg x_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>


<!-- Load the joint controllers. One of these publishes the joint states
to joint_states. -->
<node name="controller_spawner" pkg="controller_manager" type="spawner"
args="$(find mpc_model_gazebo)/config/car_like/ackermann_bot_joint_ctrlr_params.yaml"/>

<!-- Control the steering, axle, and shock absorber joints. -->
<node name="ackermann_controller" pkg="mpc_model_gazebo"
type="ackermann_controller.py">
<param name="cmd_timeout" value="$(arg cmd_timeout)"/>
<rosparam file="$(find mpc_model_gazebo)/config/car_like/ackermann_bot_ackermann_ctrlr_params.yaml" command="load"/>
</node>

<!-- ************** Map ************** -->
<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"/>
</node>

<!-- ************** Localization ************** -->

<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find mpc_ros)/params/amcl_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 *************** -->
<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" />

<!-- 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 -->
<rosparam file="$(find mpc_ros)/params/mpc_last_params.yaml" command="load" />
<param name="base_local_planner" value="mpc_ros/MPCPlannerROS" if="$(eval controller == 'mpc')"/>
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" if="$(eval controller == 'dwa')"/>

<!-- external controller >
<remap from="/cmd_vel" to="/fake_cmd" unless="$(eval controller == 'dwa')"/-->
</node>

<!-- ************** Visualisation ************** -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_ros)/params/total_rviz_navigation.rviz"/>
</launch>
77 changes: 77 additions & 0 deletions mpc_ros/launch/mpc_local_planner_holonomic.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
<launch>

<!-- ************** Global Parameters *************** -->
<param name="use_sim_time" value="true"/>
<arg name="controller" default="mpc" doc="opt: dwa, mpc, pure_pursuit"/>
<arg name="model" default="holo_bot" doc="opt: serving_bot, holo_bot"/>
<arg name="tf_prefix" default=""/>

<!-- ************** GAZEBO Simulator *************** -->
<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.507"/>
<arg name="gui" default="false"/>


<!-- ************** Robot model *************** -->
<param name="robot_description" command="$(find xacro)/xacro.py $(find mpc_model_gazebo)/urdf/holonomic.urdf.xacro" if="$(eval model == 'holo_bot')"/>

<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find mpc_ros)/worlds/sq_world.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>

<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model holonomic_bot -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>

<!-- ************** Map ************** -->
<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"/>
</node>

<!-- ************** Localization ************** -->

<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find mpc_ros)/params/amcl_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 *************** -->
<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" />

<!-- 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 -->
<rosparam file="$(find mpc_ros)/params/mpc_last_params.yaml" command="load" />
<param name="base_local_planner" value="mpc_ros/MPCPlannerROS" if="$(eval controller == 'mpc')"/>
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" if="$(eval controller == 'dwa')"/>

<!-- external controller >
<remap from="/cmd_vel" to="/fake_cmd" unless="$(eval controller == 'dwa')"/-->
</node>


<!-- ************** Visualisation ************** -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_ros)/params/total_rviz_navigation.rviz"/>
</launch>
72 changes: 72 additions & 0 deletions mpc_ros/launch/nav_gazebo_holo.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
<launch>

<!-- ************** Global Parameters *************** -->
<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"/>
<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.0"/>
<arg name="gui" default="true"/>


<!-- ************** Map ************** -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_ros)/map/big_s_world.yaml" output="screen">
<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" />
<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 *************** -->
<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" />

<!-- 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')"/>

<!-- external controller -->
<remap from="/cmd_vel" to="/fake_cmd" unless="$(eval controller == 'dwa')"/>
</node>

<!-- ************** MPC Node ************** -->
<node name="nav_mpc" pkg="mpc_ros" type="MPC_Node_holo" 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')" >
<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"/>
</launch>
Loading