Tuesday, September 11, 2018

CMake

Reference

CMake Tutorial

CMake is designed to be used in conjunction with the native build environment. configuration files placed in each source directory (called CMakeLists.txt files) are used to generate standard build files Makefile

CMakeLists.txt places

  • Source folder
  • Root of the source tree of any application, library
  • For each module that can be compile and build separately

Hello cmake

  • source
#include <iostream> int main(void) { std::cout << "Hello World" << std::endl; return(0); }
  • CMakeLists.txt (with the source folder)
# Set the minimum required version of cmake for a project cmake_minimum_required(VERSION 2.8) # Project name project(hello) # Add an executable to the project using the specified source files add_executable(hello hello.cpp)
  • Run
cmake . make

Build and bin sub folders

To keep files in place , and hold all cmake file in one place

mkdir build cd build cmake .. make

Project structure

p1 ├── bin ├── build ├── src | └── CMakeLists.txt └─ CMakeLists.txt
  • Source CMakeLists.txt
cmake_minimum_required(VERSION 2.8) project(hello) # Assign value to variable (make output folder) set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin) # Adds sub folder to build add_subdirectory(src)
  • Root src CMakeLists.txt
# Print message message ("main application") add_executable(hello test.cpp)
  • from build sub folder
cmake .. make

CMake variables

cmake-variables

  • Set variable data with set function/macro
  • Access value with ${variable_name}
set(CMAKE_CXX_STANDARD 11) message("CXX Standard:" ${CMAKE_CXX_STANDARD})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
  • Custom variables
set(TRIAL_VARIABLE "VALUE") message("${TRIAL_VARIABLE}")

Building a Library with cmake

  • Add lib/math sub folder under src
  • Add header and source file
  • Change main app to use the new functionality

code

  • header file (src/lib/math/calc.hpp)
#ifndef CALC_HPP #define CALC_HPP namespace math { class calc{ public: int sum(const int &a, const int &b); }; } #endif
  • implementation (src/lib/math/calc.cpp)
#include "calc.hpp" int math::calc::sum(const int &a, const int &b){ return a + b; }
  • main
#include <iostream> #include "lib/math/calc.hpp" using namespace std; int main(void) { math::calc calc; int sum = calc.sum(3, 4); cout << "3+4=" << sum << endl; return(0); }
  • CMakeLists.txt
cmake_minimum_required(VERSION 2.8) project(hello) # set(CMAKE_BINARY_DIR ${CMAKE_SOURCE_DIR}/bin) set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib) # CMAKE_ARCHIVE_OUTPUT_DIRECTORY set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") add_subdirectory(src)
  • CMakeLists.txt (src)
# create shared library add_library(math SHARED lib/math/calc.cpp) add_executable(hello test.cpp) # link libmath.so to executable target_link_libraries(hello math)
  • It's create shared library libxxx.so at lib sub folder
  • It's create binary use's the lib in bin sub folder

Friday, September 7, 2018

ROS REP and CodeStyle

REP - ROS Enhancement Proposal 

  • REP-103: Standard Units of Measure and Coordinate Conventions
  • REP-144: ROS Package Naming


ROS  Code Style Guide

ROS Projects Tips 

Thursday, September 6, 2018

Gazebo and URDF model

source code

Catkin package

catkin_create_pkg demo_gazebo rospy cd demo_gazebo mkdir launch mkdir urdf

Minimal urdf file

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")

Wednesday, September 5, 2018

MAVROS Part1 - Hello

  • Install mavros from source
  • Write python node that ARM and SET POSE

Install

Preparation

  • clean older / distribution packages
sudo apt purge ros-kinetic-mavros*

Install mavros from source

  • create workspace
mkdir -p ~/mavros_ws/src cd ~/mavros_ws catkin init wstool init src
  • Install MAVLink
rosinstall_generator --rosdistro kinetic mavlink | tee /tmp/mavros.rosinstall
  • Install mavros
rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
  • Create Workspace
wstool merge -t src /tmp/mavros.rosinstall wstool update -t src -j4 rosdep install --from-paths src --ignore-src -y
  • Install geolib
./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
  • Build
catkin build
  • Add to
source ~/mavros_ws/devel/setup.bash # update bashrc

Write code

  • [MAVROS offboard control example] (https://dev.px4.io/en/ros/mavros_offboard.html)
  • source

Use services and topics to control the FCU

#!/usr/bin/env python import rospy import mavros from geometry_msgs.msg import PoseStamped from mavros_msgs.msg import State from mavros_msgs.srv import CommandBool, SetMode # callback method for state sub current_state = State() offb_set_mode = SetMode def state_cb(state): global current_state current_state = state mavros.set_namespace() local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, queue_size=10) state_sub = rospy.Subscriber(mavros.get_topic('state'), State, state_cb) arming_client = rospy.ServiceProxy(mavros.get_topic('cmd', 'arming'), CommandBool) set_mode_client = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode) pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 2 def main(): rospy.init_node('offb_node', anonymous=True) prev_state = current_state rate = rospy.Rate(20.0) # MUST be more then 2Hz # send a few setpoints before starting for i in range(100): local_pos_pub.publish(pose) rate.sleep() # wait for FCU connection while not current_state.connected: rate.sleep() last_request = rospy.get_rostime() while not rospy.is_shutdown(): now = rospy.get_rostime() if current_state.mode != "OFFBOARD" and (now - last_request > rospy.Duration(5.)): set_mode_client(base_mode=0, custom_mode="OFFBOARD") last_request = now else: if not current_state.armed and (now - last_request > rospy.Duration(5.)): arming_client(True) last_request = now if prev_state.armed != current_state.armed: rospy.loginfo("Vehicle armed: {}".format(current_state.armed)) if prev_state.mode != current_state.mode: rospy.loginfo("Current mode: {}".format(current_state.mode)) prev_state = current_state # Update timestamp and publish pose pose.header.stamp = rospy.Time.now() local_pos_pub.publish(pose) rate.sleep() if __name__ == '__main__': try: main() except rospy.ROSInterruptException: pass

Usage

  • Terminale 1
roslaunch px4 posix_sitl.launch
  • Terminale 2
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"
  • Terminale 3
cd ~/mavros_ws source devel/setup.bash # The above set the PYTHONPATH env. rosrun mavros_demo offb_node.py

Tuesday, September 4, 2018

PX4 SITL mavros and gazebo

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}"

Monday, September 3, 2018

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

Sunday, September 2, 2018

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

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

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)