Commandeering Gazebo Simulation

"Containing all the needs to build a gazebo simulation"

For better look check this note, please visit HackMD.io

by:
Liber Normous

---

Tutorial from scratch part 1

• http://gazebosim.org/tutorials/?tut=ros_control
• 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.
3. Move the origin of the object to center of mass.
4. Add FCInfo to FreeCAD to analyze Inertia etc. https://www.youtube.com/watch?v=gThQk-4p1CY
   • 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
5. Save to spreadsheed

---

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 a Visual Robot Model with URDF from Scratch
• 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:

sudo apt-get install liburdfdom-tools
check_urdf FILE.urdf

Example of URDF file, 1 object, no joints:

<?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

NOTE:

• model_xml is the file. not the path

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

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

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
  $[xy\theta ]$

Add model

Preparing variables:

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

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

% 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:

% delete model sservice
obj.del_srv_cli = rossvcclient('/gazebo/delete_model');
obj.del_srv_msg = rosmessage(obj.del_srv_cli);

Calling a ROS service:"

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:

% 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:

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:

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
  $[XY\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:

% 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:"

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

---

tags: Web Clipper