---
# System prepended metadata

title: Commandeering Gazebo Simulation
tags: [Web Clipper]

---

---
slideOptions:
  transition: slide
---
<style>
.reveal {
  font-size: 24px;
}
</style>




# Commandeering Gazebo Simulation  

"Containing all the needs to build a gazebo simulation"  

For better look check this note, please visit [HackMD.io](https://hackmd.io/@libernormous/commandeering-gaebo)  

by:  
[Liber Normous](https://hackmd.io/@libernormous)  

---

# Tutorial from scratch part 1  

* {%youtube 8ckSl4MbZLg%}  
* {%youtube q01dDxeHTMs%}
* http://gazebosim.org/tutorials/?tut=ros_control  
* ![](https://i.imgur.com/0SB6mp2.png)  
* 1. Create gazebo simulation package.  
* 2. Create `gazebo_launch.launch` file that includes:    
    * Setting some parameters.  
    * Passing that parameter to open `world` file.   
        * Put in a single `.launch` file.   
* 

---

# Creating parts using FreeCAD  
**STEPS:**  
1. Create model.  
2. Add center of mass tools.  
    * ![](https://i.imgur.com/Wq28CGQ.png)  
4. Move the origin of the object to center of mass.  
5. Add `FCInfo` to FreeCAD to analyze Inertia etc.   https://www.youtube.com/watch?v=gThQk-4p1CY  
    * ![](https://i.imgur.com/lG08NjR.png)  
    * Tools Mebu Bar  
    * Addon manager  
    * Macros TAB  
    * Install FCInfo  
    * Macros Menu Bar  
    * FCInfo.FCMacro  
    * Execute  
    * Set the object density ($\rho(kg/l)$)  
    * Check `Moment of inertia with mass`  
6. Save to spreadsheed  ![](https://i.imgur.com/Xip6d2K.png)  

---

# Create URDF File  
**TEMPLATE:**
* https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro  

**Tutorial:**
* http://gazebosim.org/tutorials/?tut=ros_urdf  
* http://wiki.ros.org/urdf/Tutorials/Building%20a%20Visual%20Robot%20Model%20with%20URDF%20from%20Scratch  
* http://gazebosim.org/tutorials?tut=import_mesh  

**NOTE:**
* `MESH` needs `FILENAME` attributes.  `<mesh filename="/media/zee/53FC06C50A4891B9/Project/m-43-commadeering_gazebo/obstacles-box.dae"/>`    

**Check teh URDF FILE:**  
```bash=
sudo apt-get install liburdfdom-tools
check_urdf FILE.urdf
```
**Example of URDF file, 1 object, no joints:**  

```xml=
<?xml version="1.0"?>
<!-- Simple BOX -->
<robot name="box1" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Base Link -->
  <link name="link1">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
          <geometry>
            <mesh filename="/media/zee/53FC06C50A4891B9/Project/m-43-commadeering_gazebo/obstacles-box.dae"/>
          </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
          <geometry>
            <mesh filename="/media/zee/53FC06C50A4891B9/Project/m-43-commadeering_gazebo/obstacles-box.dae"/>
          </geometry>
      <material name="orange"/>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.525"/>
      <inertia
	  ixx="7109.374999999998" ixy="0.0" ixz="0.0"
	  iyy="16296.874999999998" iyz="0.0"
	  izz="12687.500000000002"/>
    </inertial>
  </link>

</robot>
```

---

# Create SDF file (does not work on RVIZ)  

**Template and Tutorial:** http://gazebosim.org/tutorials?tut=build_model  

---

# Terminal functionality  

## Spawning object using `gazebo` package   

[**Services: Create and destroy models in simulation**
](http://gazebosim.org/tutorials/?tut=ros_comm)  
![](https://i.imgur.com/nr4VDVt.png)  
**NOTE:**  
* `model_xml` is the file. not the path  

```bash=
rosservice call /gazebo/spawn_urdf_model "model_name: 'box1'
model_xml: '$(cat /media/zee/53FC06C50A4891B9/Project/m-43-commadeering_gazebo/box1.urdf)'
robot_namespace: ''
initial_pose:
  position: {x: 0.0, y: 0.0, z: 0.0}
  orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
reference_frame: ''"
```

---

## Deleting object `gazebo` package   
[**Services: Create and destroy models in simulation**
](http://gazebosim.org/tutorials/?tut=ros_comm)  
![](https://i.imgur.com/nr4VDVt.png)  
```bash=
rosservice call /gazebo/delete_model "model_name: 'box1'"
```

---

## Teleporting object to other location  

**NOTE:**    
* This is affecting the odometry reading!  
* Set the position and orientation. Not the TWIST. Twist will be overwritten by `/cmd_vel`   

```bash  
rosservice call /gazebo/set_model_state "model_state:
  model_name: 'neuronbot2'
  pose:
    position:
      x: -3
      y: 1.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 0.0
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  reference_frame: ''"
```

---

# MATLAB add, delete, move, getting states of models  

**NOTE**:
* This methods are inside a class. That is why we have `obj.` on every variables.  
* `model_state` input is in format of $[x\ \ y\ \ \theta]$  
 
## Add model  

**Preparing variables:**  
```c=
obj.add_srv_cli = rossvcclient('/gazebo/spawn_urdf_model');
obj.add_srv_msg = rosmessage(obj.add_srv_cli);
```

**Calling a ROS service:"**  
* Object is always drop from 1m above for simplicity  
```c=
function obj = add_model(obj,model_name, model_state, urdf_string)
    % assign message
    obj.add_srv_msg.ModelName = model_name;
    obj.add_srv_msg.ModelXml = urdf_string;
    obj.add_srv_msg.InitialPose.Position.X = model_state(1);
    obj.add_srv_msg.InitialPose.Position.Y = model_state(2);
    obj.add_srv_msg.InitialPose.Position.Z = 1;
    % OUTPUT: Q, is an N-by-4 matrix containing N quaternions.
    % Each quaternion is of the form q = [w x y z], with w as the
    % scalar number.
    % INPUT: By default, the ZYX axis order will be used.
    Q = eul2quat([model_state(3) 0 0]);
    obj.add_srv_msg.InitialPose.Orientation.W = Q(1);
    obj.add_srv_msg.InitialPose.Orientation.X = Q(2);
    obj.add_srv_msg.InitialPose.Orientation.Y = Q(3);
    obj.add_srv_msg.InitialPose.Orientation.Z = Q(4);
    %call service
    call(obj.add_srv_cli, obj.add_srv_msg);
end
```
**How to read URDF file into string in MATLAB**  
```c=
% reading urdf
f_id = fopen('/media/zee/53FC06C50A4891B9/Project/m-43-commadeering_gazebo/urdf/box1.urdf');
% Read all lines & collect in cell array
urdf_txt = fscanf(f_id,'%c'); 
```

## Delete a model  

**Preparing variables:**  
```c=
% delete model sservice
obj.del_srv_cli = rossvcclient('/gazebo/delete_model');
obj.del_srv_msg = rosmessage(obj.del_srv_cli);
```

**Calling a ROS service:"**  

```c=
function obj = del_model(obj,model_name)
    % assign message
    obj.del_srv_msg.ModelName = model_name;
    %call service
    call(obj.del_srv_cli, obj.del_srv_msg);
end
```

## Get model states  

**NOTE:**  
* We Get states not from ***service***. But from Subscribing into a ***topic***.  
* This is done because we need to update the states everytime and the easiest way is using `callback` function.  
* Callback function will update the states whenever there is a state change.  
* Iside the callback function `delay` or `pause` is necessary. `¯\_(ツ)_/¯
`.  

**Preparing variables:**  
```c=
% subscribe to model states
disp('subscribing /gazebo/model_states: pause 2s');
obj.model_states_msg = rosmessage('gazebo_msgs/ModelStates');
pause(2);
obj.model_states_sub = rossubscriber('/gazebo/model_states',@obj.model_states_cb); 
```

**Extracting message from the topic:**  

```c=
function state_val = get_state(obj,model_name)
    index = strcmp(obj.model_names,model_name);
    state_X = obj.model_states_msg.Pose(index).Position.X;
    state_Y = obj.model_states_msg.Pose(index).Position.Y;
    state_roll = obj.model_states_msg.Pose(index).Orientation.X;
    state_pitch = obj.model_states_msg.Pose(index).Orientation.Y;
    state_yaw = obj.model_states_msg.Pose(index).Orientation.Z;
    state_mag = obj.model_states_msg.Pose(index).Orientation.W;
    % INPUT: The default rotation sequence is q = [w x y z]
    % OUTPUT: The default rotation sequence is 'ZYX'
    eul = quat2eul([state_mag state_roll state_pitch state_yaw]);
    state_val=[state_X state_Y eul(1)];
end
```

**Call back function:**  
```c=
function obj = model_states_cb(obj,~,message)
    obj.model_states_msg = message;
    obj.model_names = obj.model_states_msg.Name;
    pause(0.05);
end
```

## Move a model  

**NOTE**  
* Moving here is limited to $[X\ \ Y\ \ \theta]$ dirrection for simplicity.  
* To move, we need to know the current $Z$ axis position value.  
* We get $Z$ axis position value from subscribing to `'/gazebo/model_states'`.  
* 

**Preparing variables:**  
```c=
% set state model service
obj.set_srv_cli = rossvcclient('/gazebo/set_model_state');
obj.set_srv_msg = rosmessage(obj.set_srv_cli);
```

**Calling a ROS service:"**  
```c=
function obj = set_state(obj, model_name, model_state)
    index = strcmp(obj.model_names,model_name);
    % assign message
    obj.set_srv_msg.ModelState.ModelName = model_name;
    % assign message
    obj.set_srv_msg.ModelState.ModelName = model_name;
    obj.set_srv_msg.ModelState.Pose.Position.X = model_state(1);
    obj.set_srv_msg.ModelState.Pose.Position.Y = model_state(2);
    obj.set_srv_msg.ModelState.Pose.Position.Z = obj.model_states_msg.Pose(index).Position.Z;
    % OUTPUT: Q, is an N-by-4 matrix containing N quaternions.
    % Each quaternion is of the form q = [w x y z], with w as the
    % scalar number.
    % INPUT: By default, the ZYX axis order will be used.
    Q = eul2quat([model_state(3) 0 0]);
    obj.set_srv_msg.ModelState.Pose.Orientation.W = Q(1);
    obj.set_srv_msg.ModelState.Pose.Orientation.X = Q(2);
    obj.set_srv_msg.ModelState.Pose.Orientation.Y = Q(3);
    obj.set_srv_msg.ModelState.Pose.Orientation.Z = Q(4);
    %call service
    call(obj.set_srv_cli, obj.set_srv_msg);            
end
```

![](https://i.imgur.com/ceTp14s.png)  

![](https://i.imgur.com/eQ0vdHA.png)  
![](https://i.imgur.com/volFN51.png)  


---

###### tags: `Web Clipper`