Gazebo and URDF model

source code

Catkin package

catkin_create_pkg demo_gazebo rospy
cd demo_gazebo
mkdir launch
mkdir urdf

Minimal urdf file

URDF model link mast have inertia

The inertia matrix explained

<?xml version="1.0"?>
<robot name="robot">
    <link name="base_link">
      <collision>
      <origin xyz="0 0 1" rpy="0 0 0"/>
      <geometry>
        <box size="2 2 2"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 1" rpy="0 0 0"/>
      <geometry>
        <box size="2 2 2"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <inertial>
      <origin xyz="0 0 1" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia
        ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0"/>
    </inertial>>
    </link>
</robot>

Spawn models

• Using launch file

<launch>
    <param name="robot_description" textfile="$(find demo_gazebo)/urdf/robot_description.urdf" />
    <include file="$(find gazebo_ros)/launch/empty_world.launch"/>
    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"  args="-param robot_description -urdf -model my_robot" />
</launch>

• Using rosrun

rosrun gazebo_ros spawn_model -file `rospack find demo_gazebo`/urdf/robot_description.urdf -urdf -z 1 -model my_robot

Gazebo services

• Check if mode loaded

rosservice call /gazebo/get_world_properties 

sim_time: 1155.376
model_names: [ground_plane, my_robot]
rendering_enabled: True
success: True

• Delete model

rosservice call /gazebo/delete_model "model_name: 'my_robot'" 

success: True
status_message: "DeleteModel: successfully deleted model"

ROS python node

• Call service to insert model into

#! /usr/bin/env python

import rospy
from gazebo_msgs.srv import SpawnModel
from geometry_msgs.msg import Pose


rospy.init_node('insert_object',log_level=rospy.INFO)

initial_pose = Pose()
initial_pose.position.x = 1
initial_pose.position.y = 1
initial_pose.position.z = 1

f = open('/home/user/simulation_ws/src/demo_gazebo/urdf/robot_description.urdf','r')
urdff = f.read()

rospy.wait_for_service('gazebo/spawn_urdf_model')
spawn_model_proxy = rospy.ServiceProxy('gazebo/spawn_urdf_model', SpawnModel)
spawn_model_proxy("some_robot_name", urdff, "robotos_name_space", initial_pose, "world")

PX4 SITL mavros and gazebo

• Reference
• 404warehouse

Setup and run

• Terminal 1 Setting environment

cd <Firmware_clone>
make posix_sitl_default gazebo

source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/posix_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo

• Terminal 1 (launch gazebo and SITL)

roslaunch px4 posix_sitl.launch

• Terminal 2 (launch mavros)

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

• Terminal 3 (mavros commands: call services)

rosservice call /mavros/set_mode "base_mode: 0
custom_mode: 'OFFBOARD'" 

rosservice call /mavros/cmd/arming "value: true" 

rosservice call /mavros/cmd/takeoff "{min_pitch: 0.0, yaw: 0.0, latitude: 0.0, longitude: 0.0, altitude: 10.0}" 

Pixhawk first connection

• Connect to usb
• Understand LED's
• Write some dronekit code read heartbeat
• Read GPS data with dronekit

Connect to USB

• dmsg

[768651.390811] usb 3-2: Product: PX4 FMU v2.x
[768651.390814] usb 3-2: Manufacturer: 3D Robotics
[768651.390816] usb 3-2: SerialNumber: 0
[768651.391528] cdc_acm 3-2:1.0: ttyACM0: USB ACM device

LED's

• Flashing blue: Disarmed, no GPS lock found
• Flashing green: Disarmed (ready to arm), GPS lock acquired.
• Full list: http://ardupilot.org/plane/docs/common-leds-pixhawk.html

Connect GPS and Compass

• LED go to blinking green
• my GPS led blink with green led's

Dronekit hello

#!/usr/bin/env python
from dronekit import connect, VehicleMode

connection_string = "/dev/ttyACM0"

# Connect to the Vehicle.
print("Connecting to vehicle on: %s" % (connection_string,))
vehicle = connect(connection_string, wait_ready=True)

# Get some vehicle attributes (state)
print "Get some vehicle attribute values:"
print " GPS: %s" % vehicle.gps_0
print " Battery: %s" % vehicle.battery
print " Last Heartbeat: %s" % vehicle.last_heartbeat
print " Is Armable?: %s" % vehicle.is_armable
print " System status: %s" % vehicle.system_status.state
print " Mode: %s" % vehicle.mode.name    # settable

# Close vehicle object before exiting script
vehicle.close()

Read GPS data

#!/usr/bin/env python
from dronekit import connect, VehicleMode
import time

connection_string = "/dev/ttyACM0"

def log_gps(self, name, msg):
    print msg

# Connect to the Vehicle.
print("Connecting to vehicle on: %s" % (connection_string,))
vehicle = connect(connection_string, wait_ready=True)

#vehicle.add_message_listener('GLOBAL_POSITION_INT', log_gps)
vehicle.add_message_listener('GPS_RAW_INT', log_gps)

time.sleep(5)
vehicle.close()

Result

• GPS_RAW_INT: The global position, as returned by the Global Positioning System (GPS). This is NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame)

GPS_RAW_INT {time_usec : 4246687000, fix_type : 4, lat : 324283000, lon : 350107200, alt : 45700, eph : 98, epv : 171, vel : 14, cog : 23071, satellites_visible : 8}

• GLOBAL_POSITION_INT: fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up)

GLOBAL_POSITION_INT {time_boot_ms : 2848710, lat : 324284000, lon : 350107830, alt : 99400, relative_alt : 8000, vx : -17, vy : 9, vz : 8, hdg : 4893}

Reference

• Pixhawk Overview
• LEDs (Pixhawk)
• GLOBAL_POSITION_INT

Ardupilot and MAVROS

Install MAVROS

sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras

• GCS — Ground Control Station
• FCU — Flight Control Unit (pixhawk)
• OBC — OnBoard Computer (your odroid or raspberry)

Run SITL and ROS

Terminal 1 (sitl)

sim_vehicle.py -v ArduCopter --console

Terminal 2 (launch)

udp://[bind_host][:port]@[remote_host[:port]][/?ids=sysid,compid]

roslaunch mavros apm.launch fcu_url:=udp://127.0.0.1:14551@14555

rqt (Terminal 3)

Use GUI to control (call service, monitor and pub topics)

rqt

• plugins -> robots tools -> runtime monitor

Set mode

• plugins -> Services -> Service Caller
• from service combo select /mavros/set_mode

Reference

• mavros
• ROS with SITL

Mission Planner VirtualBOX Dronekit-sitl

MissionPlaner on virtualbox

Mission Planer setup

• Host: ubuntu 16.04
• Guest: Windows7
• Virtualbox
• Install VM VirtualBox Extension Pack (support USB2,3)
• Install VB Guest Addition
• Install Firfox (IE crash all the time)
• .Net 4.62
• Mission Planner
• Set VBOX network in bridge mode

Demo: Using Dronekit and Dronekit-SITL

• Reference

sudo pip install mavproxy
sudo pip install dronekit
sudo pip install dronekit-sitl

• Terminal 1

dronekit-sitl copter

• Terminal 2

export MP_IP="192.168.2.117"

mavproxy.py --master tcp:127.0.0.1:5760 --out udp:127.0.0.1:14551 --out udp:`echo $MP_IP`:14550

• Virtualbox (windows)
  • Change the connection type combo to udp
  • Select port 14550

Note

When run mavproxy got an error: AttributeError: 'module' object has no attribute 'MAV_TYPE_DODECAROTOR'

After some search in google , I found that pymavlink need to upgraded , download and install from pypi.org version 2.2.10 (current version 2.0.6)

Setting Title for Tab in Terminator

Add code to ~/.bashrc

Run source ~/.bashrc

Usage: set_title <Title>

source

set_title() {
ORIG=$PS1
TITLE="\e]2;$*\a"
PS1=${ORIG}${TITLE}
}


Reference 

ROS LIVE_CLASS

Start to follow and implement LIVE CLASS from "The Construct"
Ricardo are a greate and funny guide

#13:ROS Navigation Stack How To

In this ROS LIVE-Class Ricardo show how to
making a differential drive robot create a map (SLAM), localize on that map (robot localization), and then navigate on the environment using the map (path planning and obstacle avoidance).

• Class#13 
• My Source

#14: How To Control Robot Joints With ROS

In this ROS LIVE-Class Ricardo show how to add ROS control to a joint in a simulated robot

• Class#14

#19: Let’s Use Gazebo Plugins

In this ROS LIVE-Class Ricardo show how to create a Gazebo plugin for a robot simulated world. The plugin will allow us to connect/disconnect the light of the world by means of a ROS topic .

• Class#19
• My source