Monday, October 22, 2018


Bash Tips

Use arguments from previous command

!^ first argument !$ last argument !* all arguments !:2 second argument !:2-3 second to third arguments !:2-$ second to last arguments !:2* second to last arguments !:2- second to next to last arguments !:0 the command !! repeat the previous line

Using first argument

echo a b c d a b c d echo !:1 echo a a

Using last argument

echo a b c d a b c d echo !$ echo d d

Sunday, October 21, 2018

VSCode cmake and debug

VSCode build with cmake

  • ctrl+shift+p -> Tasks: Configure Task

vscode open tasks.json

  • Project fs structure
├── bin ├── build ├── doc └── src ├── CMakeLists.txt └── hello.cpp └── CMakeLists.txt
  • Add three task
    • cmake: run cmake from build sub folder
    • make: run make depend on cmake
    • build: bind the above
{ "version": "2.0.0", "tasks": [ { "label": "cmake", "type": "shell", "options": { "cwd": "${workspaceRoot}/build" }, "command": "cmake", "args": [ ".." ] }, { "label": "make", "type": "shell", "options": { "cwd": "${workspaceRoot}/build" }, "command": "make", "dependsOn": "cmake" }, { "label": "build", "type": "shell", "dependsOn": "make", "command": "echo", "group": { "kind": "build", "isDefault": true } } ] }
  • Note: task build command key need to be fill with some value echo

cmake files

  • root cmake
    • set bin folder as binary output
    • set c++ version: set(CMAKE_CXX_STANDARD 11)
cmake_minimum_required(VERSION 3.0) project(tutorials) #set binary output set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin) message(${EXECUTABLE_OUTPUT_PATH}) #set flags set(CMAKE_CXX_STANDARD 11) #add source directory add_subdirectory(src)
  • src
add_executable(hello hello.cpp)

Debug Task

  • Control cmake settings from command line
  • Add args to task
  • Add option variable and if statement to cmake

Main CMakeLists.txt

  • Add boolean variable control
option (MODE_DEBUG "Set debug mode" OFF) if (MODE_DEBUG) message("DEBUG MODE") set (CMAKE_BUILD_TYPE Debug) endif()
  • Add new task to VSC with args -DMODE_DEBUG=TRUE
{ "label": "cmake_with_debug", "type": "shell", "options": { "cwd": "${workspaceRoot}/build" }, "command": "cmake", "args": [ "..", "-DMODE_DEBUG=TRUE" ] }
  • Add VSC keybinding
{ "key": "ctrl+shift+d", "command": "workbench.action.tasks.runTask", "args": "cmake_with_debug" }



  • Changed to debug mode
  • From combo box -> Add configuration
    • open launch.json
  • Set program to debug
"program": "${workspaceFolder}/bin/hello",

Saturday, October 20, 2018

cross-compile strace tool for ARM

Compile strace statically build use on RPi

- Download strace source code strace 4.8
- Download/Pull RPi toolchain:
- Export CC variable point to compiler

export CC=/tools/arm-bcm2708/arm-rpi-4.9.3-linux-gnueabihf/bin/arm-linux-gnueabihf-gcc


./configure --host=arm-linux -LDFLAGS=-static


file strace
strace: ELF 32-bit LSB executable, ARM, EABI5 version 1 (SYSV), statically linked, for GNU/Linux 2.6.32, not stripped

Friday, October 12, 2018

ROS Follow the line

Gazebo image topics

  • Terminal1 (gazebo turtlebot)
roslaunch turtlebot_gazebo turtlebot_world.launch
  • Terminal2
rostopic list
  • depth camera
  • rgb camera
    • raw: raw image
    • compress: compress image
    • theora: compress video

ROS sent image useing sensor_msgs/Image

Follower node

  • subscribe to /camera/rgb/image_raw topic receive Image msg
#!/usr/bin/env python import rospy from sensor_msgs.msg import Image def image_callback(msg): pass def main(): rospy.init_node("follower") rospy.loginfo("start node") sub = rospy.Subscriber("/camera/rgb/image_raw", Image, image_callback) while not rospy.is_shutdown(): rospy.spin() if __name__ == "__main__": main()
  • Set node permission (chmod +x)
  • Run in separated Terminal
rosrun <package_name>
  • Check that the node is running
  • Get node info with rosnode info /follower
  • Check message receive interval
rostopic hz /camera/rgb/image_raw subscribed to [/camera/rgb/image_raw] WARNING: may be using simulated time average rate: 30.208 min: 0.010s max: 0.060s std dev: 0.01642s window: 30 average rate: 28.061 min: 0.010s max: 0.060s std dev: 0.01638s window: 56

Setup follow the line

launch file and projects structure

  • launch file
    • worlds
  • gazebo resource
    • materials
├── launch │   └── follow.launch ├── materials │   ├── course.material │   └── course.png ├── models ├── scripts │   └── ├── src └── worlds └──
<?xml version="1.0"?> <launch> <env name="GAZEBO_RESOURCE_PATH" value="$(find follow_bot)/materials:$(optenv GAZEBO_RESOURCE_PATH)" /> <include file="$(find gazebo_ros)/launch/empty_world.launch"> <arg name="world_name" value="$(find follow_bot)/worlds/"/> </include> <include file="$(find turtlebot_gazebo)/launch/includes/kobuki.launch.xml"> <arg name="base" value="kobuki"/> <arg name="stacks" value="hexagons"/> <arg name="3d_sensor" value="kinect"/> </include> </launch>

Note: launch file set GAZEBO_RESOURCE_PATH environment variable using env tag

View image

#!/usr/bin/env python import rospy from sensor_msgs.msg import Image import cv2 import cv_bridge class Follower(object): def __init__(self): self._bridge = cv_bridge.CvBridge() sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback) def image_callback(self, msg): image = self._bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8') cv2.imshow("window", image) cv2.waitKey(3) def on_shutdown(): rospy.loginfo("shutdown") cv2.destroyAllWindows() def main(): rospy.init_node("follower") rospy.loginfo("start node") rospy.on_shutdown(on_shutdown) while not rospy.is_shutdown(): follower = Follower() rospy.spin() if __name__ == "__main__": main()

Note: When we use the cv2.namedWindow Method the Window was freeze check the line solution

Detecting the line

  • Find the yellow line
lower_yellow = numpy.array([ 50, 50, 170]) upper_yellow = numpy.array([255, 255, 190]) mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
  • left: orignal
  • center: mask imaged (binary)
  • right: masked image (extrat only the line from orignal image)

Find line center

  • Mask image and take only relevant part (closer path front to the robot)

  • Find line center with cv2 moments method
  • For debug draw circle on line centroid
M = cv2.moments(mask) if M['m00'] > 0: cx = int(M['m10']/M['m00']) cy = int(M['m01']/M['m00']), (cx, cy), 20, (0,0,255), -1)

- Public `Twist` message , calc z angular value ```python #delta/ error between line center to image center delta = cx - w/2 twist = Twist() twist.linear.x = 0.2 twist.angular.z = -float(delta) / 100 self._cmd_vel_pub.publish(twist)

Final demo


Tuesday, October 9, 2018

ROS OpenCV hello

ros kinetic work with openCV 3.1

sudo apt install ros-kinetic-opencv3
  • after install check with python
> import cv2 > cv2.__version__ '3.3.1-dev'
  • Install helper utils and packages
sudo apt install ros-kinetic-usb-cam sudo apt install ros-kinetic-image-view


Convert ROS image messages (sensor_msgs/Images) and OpenCv images (cv::Mat)


  • Use as Gazebo camera plugin post as Image source
  • Create package named cv_demo run the source code as rosnode package dependencies: sensor_msgs, opencv2, cv_bridge, rospy
  • The node create OpenCv image window
  • Use rqt to view publish image image_topic

Demo setup

  • Terminal 1 (gazebo as image source)
roslaunch gazebo_ros empty_world.launch
  • Terminal 2 (spawn camera)
rosrun gazebo_ros spawn_model -file camera.urdf -urdf -model bot
  • Termianl 3 (run our node)
rosrun cv_demo
  • Termianl 4 (rqt)


Source code

#!/usr/bin/env python import rospy import cv2 from cv_bridge import CvBridge, CvBridgeError import numpy as np from sensor_msgs.msg import Image, CameraInfo class CvBridgeDemo(object): NODE_NAME = "cv_demo" WIN_NAME = "cv_demo_win" def __init__(self): self._bridge = CvBridge() self.image_sub = rospy.Subscriber("/bot/camera/image_raw", Image, self.image_callback) self.image_pub = rospy.Publisher("image_topic", Image) rospy.loginfo("watting for image") def image_callback(self, ros_image): # Use cv_bridge() to convert the ROS image to OpenCV format bridge = self._bridge try: frame = bridge.imgmsg_to_cv2(ros_image, "bgr8") except CvBridgeError, e: rospy.logerr("fail convert ros image to cv: {}".format(e)) # Convert the image to a Numpy array for cv2 functions frame = np.array(frame, dtype=np.uint8) # Process the frame using the process_image() function display_image = self.process_image(frame) # Display the image. cv2.imshow(CvBridgeDemo.WIN_NAME, display_image) # Process any keyboard commands cv2.waitKey(3) try: self.image_pub.publish(self._bridge.cv2_to_imgmsg(display_image, "mono8")) except CvBridgeError as e: rospy.logerr("fail convert cv image to rosmsg: {}".format(e)) def process_image(self, frame): img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(img,'OpenCV',(10,500), font, 4,(255,255,255),2,cv2.LINE_AA) return img def on_shutdown(): print "Shutting down cvnode." cv2.destroyAllWindows() def main(): rospy.init_node(CvBridgeDemo.NODE_NAME) rospy.on_shutdown(on_shutdown) while not rospy.is_shutdown(): CvBridgeDemo() rospy.loginfo("wait for shutdown") #The spin() code simply sleeps until the is_shutdown() flag is True. # It is mainly used to prevent your Python Main thread from exiting. rospy.spin() if __name__ == '__main__': main()

Sunday, October 7, 2018

Gazebo camera plugin


Plugins can be added to any of the main elements of a URDF - a <robot>, <link>, or <joint> depending on what the scope and purpose of the plugin is. when add <plugin> to urdf we wrap it with <gazebo> element

<gazebo reference="<link_name>"> <plugin name="camera_controller" filename=""> ... plugin parameters ... </plugin> </gazebo>

Gazebo Camera

<gazebo reference="camera_link"> <sensor type="camera" name="camera"> <update_rate>30.0</update_rate> <camera name="head"> <horizontal_fov>1.3962634</horizontal_fov> <image> <width>800</width> <height>800</height> <format>R8G8B8</format> </image> <clip> <near>0.02</near> <far>300</far> </clip> <noise> <type>gaussian</type> <mean>0.0</mean> <stddev>0.007</stddev> </noise> </camera> <plugin name="camera_controller" filename=""> <alwaysOn>true</alwaysOn> <updateRate>0.0</updateRate> <cameraName>bot/camera</cameraName> <imageTopicName>image_raw</imageTopicName> <cameraInfoTopicName>camera_info</cameraInfoTopicName> <frameName>camera_link</frameName> <hackBaseline>0.07</hackBaseline> <distortionK1>0.0</distortionK1> <distortionK2>0.0</distortionK2> <distortionK3>0.0</distortionK3> <distortionT1>0.0</distortionT1> <distortionT2>0.0</distortionT2> </plugin> </sensor> </gazebo> </robot>

Name and Topic

<sensor type="camera" name="camera">
  • sensor name must be unique and used by cameraName
<cameraName>bot/camera</cameraName> <imageTopicName>image_raw</imageTopicName> <cameraInfoTopicName>camera_info</cameraInfoTopicName>

rostopic list

rostopic list /bot/camera/camera_info /bot/camera/image_raw /bot/camera/image_raw/compressed /bot/camera/image_raw/compressed/parameter_descriptions /bot/camera/image_raw/compressed/parameter_updates /bot/camera/image_raw/compressedDepth /bot/camera/image_raw/compressedDepth/parameter_descriptions /bot/camera/image_raw/compressedDepth/parameter_updates /bot/camera/image_raw/theora /bot/camera/image_raw/theora/parameter_descriptions /bot/camera/image_raw/theora/parameter_updates /bot/camera/parameter_descriptions /bot/camera/parameter_updates ...


The coordinate frame the image is published under in the tf tree.



  • Terminal 1(launch gazebo)
roslaunch gazebo_ros empty_world.launch
  • Terminal 2(spawn)
#from urdf path rosrun gazebo_ros spawn_model -file camera.urdf -urdf -model bot
  • Terminal 3(rqt)
rqt # Plugins -> Visualization-> image viewer # From combo /bot/camera/image_raw
  • From gazebo add model in the front of camera

  • Check view in rqt

Full urdf code

<?xml version="1.0"?> <robot name="bot"> <!-- Camera --> <link name="camera_link"> <collision> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> <box size="0.05 0.05 0.05"/> </geometry> </collision> <visual> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> <box size="0.05 0.05 0.05"/> </geometry> <material name="red"/> </visual> <inertial> <mass value="1e-5" /> <origin xyz="0 0 0" rpy="0 0 0"/> <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" /> </inertial> </link> <gazebo reference="camera_link"> <sensor type="camera" name="camera"> <update_rate>30.0</update_rate> <camera name="head"> <horizontal_fov>1.3962634</horizontal_fov> <image> <width>800</width> <height>800</height> <format>R8G8B8</format> </image> <clip> <near>0.02</near> <far>300</far> </clip> <noise> <type>gaussian</type> <mean>0.0</mean> <stddev>0.007</stddev> </noise> </camera> <plugin name="camera_controller" filename=""> <alwaysOn>true</alwaysOn> <updateRate>0.0</updateRate> <cameraName>bot/camera</cameraName> <imageTopicName>image_raw</imageTopicName> <cameraInfoTopicName>camera_info</cameraInfoTopicName> <frameName>camera_link</frameName> <hackBaseline>0.07</hackBaseline> <distortionK1>0.0</distortionK1> <distortionK2>0.0</distortionK2> <distortionK3>0.0</distortionK3> <distortionT1>0.0</distortionT1> <distortionT2>0.0</distortionT2> </plugin> </sensor> </gazebo> </robot>

Ardupilot gazebo

Gazebo Plugin for Arducopter

  • ubuntu 16.04
  • ROS Kinetic
    • gazebo 7


sudo apt-get install libgazebo7-dev

Install and build plugin

cd <path>/ardupilot git clone cd ardupilot_gazebo/ mkdir build cd build cmake .. make -j4 sudo make install


  • Add worlds to gazebo resource search path
  • Add modules to gazebo model search path (.bashrc)
#Add to .bashrc # init / export gazebo env variables source /usr/share/gazebo/ export GAZEBO_RESOURCE_PATH=/ardupilot_gazebo/gazebo_worlds:${GAZEBO_RESOURCE_PATH} #modules export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/gazebo_models:${GAZEBO_MODEL_PATH} source ~/.bashrc


  • Terminal 1 (SITL) -v ArduCopter -f gazebo-iris -m --mav10 --map --console -I0
  • v: Vehicle

  • f: Frame

  • m: mavproxy args

    • mav10
    • map
    • console
  • I:

  • Terminal 2 (gazebo)

gazebo --verbose
[Dbg] [] ArduCopter controller online detected.

Tuesday, October 2, 2018

RPi MAVROS Pixhawk connection

RPi 3+ Setup


  • Download preinstall ROS and OpenCV on Raspbian from ROSbots
  • Install (Etcher)[] Flash OS images to SD cards & USB drives, safely and easily
  • Flash the image

Enabled ssh

  • In the boot partition
  • Paste empty file named ssh

Enabled wireless

  • In the rootfs partition goto /etc/wpa_supplicant
cd <rootfs>/etc/wpa_supplicant sudo vim wpa_supplicant.conf
  • Paste and edit at the end of the file
network={ ssid="The_ESSID_from_earlier" psk="Your_wifi_password" }

First boot

  • Connect with ssh
    • user:pi password:rosbots!

Pi config

  • change locals
  • changed password
  • changed rootfs partition size
sudo raspi-config

Resize swap file

sudo vim /etc/dphys-swapfile # update CONF_SWAPSIZE=1024 # save and exit sudo /etc/init.d/dphys-swapfile stop sudo /etc/init.d/dphys-swapfile start #check free -m

Install mavlink & mavros from source

Use wstool utility for retrieve source Use catkin tool for build

sudo apt-get install python-catkin-tools python-rosinstall-generator
sudo apt-get install libgeographic-dev sudo apt-get install geographiclib-tools sudo apt install python-future
  • Create catkin workspace
mkdir - p ~/catkin_ws/src cd catkin_ws catkin init
  • Crate MAVLink and MAVROS install instruction
# Mavlink rosinstall_generator --rosdistro kinetic mavlink | tee /tmp/mavros.rosinstall #MAVROS rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
wstool merge -t src /tmp/mavros.rosinstall wstool update -t src -j4 rosdep install --from-paths src --ignore-src -y
  • Got Error
ERROR: the following packages/stacks could not have their rosdep keys resolved to system dependencies: mavros: No definition of [geographic_msgs] for OS [debian] mavros_extras: No definition of [tf] for OS [debian] test_mavros: No definition of [control_toolbox] for OS [debian] mavros_msgs: No definition of [geographic_msgs] for OS [debian]
  • Install dependencies (run rosinstall_generator for each of the packages)
  • Todo: dependency resolve ?
rosinstall_generator --rosdistro kinetic $XXX | tee -a /tmp/mavros.rosinstall geographic_msgs uuid_msgs nav_msgs tf control_toolbox actionlib_msgs realtime_tools diagnostic_msgs urdf eigen_conversions tf2_ros dynamic_reconfigure tf2_eigen rosconsole_bridge orocos_kdl visualization_msgs angles tf2_py tf2 tf2_msgs urdf_parser_plugin diagnostic_updater control_msgs trajectory_msgs actionlib rosbag_migration_rule
  • Run again
wstool merge -t src /tmp/mavros.rosinstall wstool update -t src -j4 rosdep install --from-paths src --ignore-src -y
  • Build
catkin build
  • todo: Installer script
files=() for file_name in files do rosinstall_generator --rosdistro kinetic $file_name | tee -a /tmp/mavros.rosinstall done

Mavros_extra compile error

tf2_eigen/tf2_eigen.h: No such file or directory #include <tf2_eigen/tf2_eigen.h>
  • check if tf2_eigen compile
Starting >>> tf2_eigen Starting >>> tf2_py Finished <<< mavros_msgs [ 11.3 seconds ] Finished <<< tf2_eigen [ 0.9 seconds ]
  • Add tf2_eigen to find_package (CMakeLists.txt)
find_package(catkin REQUIRED COMPONENTS mavros roscpp mavros_msgs sensor_msgs geometry_msgs std_msgs visualization_msgs urdf tf tf2_eigen )

install GeographicLib

- Download from git `` - Run it

Connect with USB

  • Connect pixhawk usb to RPI usb
  • Use default fcu_url "/dev/ttyACM0:57600"
  • Note Don't forget to source (add to .bashrc)
  • From RPi run roslaunch mavros apm.launch
  • Run diagnostic
rostopic echo -n1 /diagnostics level: 0 name: "mavros: Heartbeat" message: "Normal" hardware_id: "/dev/ttyACM0:57600" values: - key: "Heartbeats since startup" value: "10" - key: "Frequency (Hz)" value: "0.948061" - key: "Vehicle type" value: "Quadrotor" - key: "Autopilot type" value: "ArduPilotMega / ArduCopter" - key: "Mode" value: "GUIDED" - key: "System status" value: "Standby"
  • Note: if No GPS data
name: "mavros: GPS" message: "3D fix" hardware_id: "/dev/ttyACM0:57600" values: - key: "Satellites visible" value: "0"
  • Run rosrun mavros mavsys rate --all 10 (need configure stream rate parameters.)

Todo: connect Serial