# PEV-Simulation

### 1. Install turtlebot3
==See Reference 7-1, Chapter 10.5==

---

### 2. TurtleBot3 Simulation using RViz
==See Reference 7-1, Chapter 10.8==

:::danger
:bulb: **Remeber to add this line to .bashrc：** export TURTLEBOT3_MODEL=burger
:::

---

### 3. Install GAZEBO-ros
http://gazebosim.org/tutorials?tut=ros_installing#InstallROS

---

### 4. TurtleBot3 Simulation using Gazebo
==See Reference 7-1, Chapter 10.9==

:::info
**When you meet the same error below:**
[ERROR: cannot launch node of type [gazebo_ros/gzserver]: can't locate node [gzserver] in package [gazebo_ros]
ERROR: cannot launch node of type [gazebo_ros/gzclient]: can't locate node [gzclient] in package [gazebo_ros]
ERROR: cannot launch node of type [gazebo_ros/spawn_model]: can't locate node [spawn_model] in package [gazebo_ros]](http://answers.ros.org/question/245030/error-while-spawning-my-robot-in-gazebo-using-gazebo_ros/)

:bulb: Delete manually a folder called "gazebo_ros" in the "catking/devel/shared" path and then catkin_make.
:::

==Turtlebot3 - Virtual SLAM and Navigation==


---

### 5. Replace DWA_planner to TEB_planner (on turtlebot3 simulation)

==new_navigation.launch==

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="map_file" default="$(find turtlebot3_navigation)/maps/map.yaml"/>
  <arg name="open_rviz" default="true"/>
  <arg name="move_forward_only" default="false"/>

  <!-- Turtlebot3 -->
  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  <!-- Map server -->
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>

  <!-- AMCL -->
  <include file="$(find turtlebot3_navigation)/launch/amcl.launch"/>

  <!-- move_base -->
  <include file="$(find turtlebot3_navigation)/launch/new_move_base.launch">
    <arg name="model" value="$(arg model)" />
    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
  </include>

  <!-- rviz -->
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
  </group>  
</launch>

---

==new_move_base.launch==

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/>

  <!-- move_base (teb) -->
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/teb_local_planner_params_$(arg model).yaml" command="load" />

    <!-- <rosparam file="$(find my_robot_name_2dnav)/base_local_planner_params.yaml" command="load" /> -->
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>

   </node> 
</launch>

---
==**dwa_planner** on turtlebot3 **waffle**==
{%youtube v3Q6QddTTxQ %}

---
==**teb_planner** on turtlebot3 **waffle**==

---
==**teb_planner** on turtlebot3 **burger**==


---

### 6. Indicate tag point simulation

#include <ros/ros.h>
#include <iostream>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Point.h>
#include <move_base_msgs/MoveBaseGoal.h>


using namespace std;

ros::Subscriber path_sub_;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

move_base_msgs::MoveBaseGoal GOAL_;


void Path_Callback(const geometry_msgs::Point& goal)
{
  std::cout << "path callback : " << goal.x << '\n';
  std::cout << "path callback : " << goal.y << '\n';


 
  //we'll send a goal to the robot to move 1 meter forward
  GOAL_.target_pose.header.frame_id = "base_link";
  GOAL_.target_pose.header.stamp = ros::Time::now();
 
  GOAL_.target_pose.pose.position.x = goal.x;
  GOAL_.target_pose.pose.position.y = goal.y;
  GOAL_.target_pose.pose.orientation.w = 1.0;

  //tell the action client that we want to spin a thread by default
  MoveBaseClient ac("move_base", true);

  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  ROS_INFO("Sending goal");
  ac.sendGoal(GOAL_);
 
  ac.waitForResult();
 
  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Yeah!! the base moved ");
  else
    ROS_INFO("The base failed to move for some reason");



}

int main(int argc, char** argv){
  ros::init(argc, argv, "simple_navigation_goals");
  ros::NodeHandle nh;
	ros::NodeHandle nh_private("~");


 	path_sub_ = nh.subscribe("new_following_path", 10, Path_Callback);

  ros::spin();

  return 0;
}

==first try==
{%youtube 7QmwzT5z3GY %}

---

==Indicate point (with dwa_planner)==
{%youtube 2i0tBu9ug9c %}

---

==Indicate point (with teb_planner)==
{%youtube tWmSf36yvtk %}
:::danger
:bulb: **Bug：** teb_planner got stucked in a narrow hallway
:::

---

==Optomize Teb_planner==
{%youtube zd7IwCFnifI %}
{%youtube j9mcKZYIp0c %}

---
### 7. Reference

##### 7-1. [ROS Robot Programming (Powered by Turtlebot3 developers)](http://www.pishrobot.com/wp-content/uploads/2018/02/ROS-robot-programming-book-by-turtlebo3-developers-EN.pdf)

##### 7-2.