ROS Service client

• Terminal 1

roscore

• Terminal 2

rosrun turtlesim turtlesim_node

• Terminal 3

rosservice list

/clear
/kill
/reset
/rosout/get_loggers
/spawn
/turtle1/set_pen
/turtle1/teleport_absolute
/turtle1/teleport_relative
/turtlesim/get_loggers

spawn service

Spawn another turtle with spawn service

• rosservice info <service name>

rosservice info /spawn

Node: /turtlesim
URI: rosrpc://user-N56VM:43083
Type: turtlesim/Spawn
Args: x y theta name

• rossrv info <service type>

float32 x
float32 y
float32 theta
string name
---
string name

call / execute

#left down corner: 0,0
rosservice call /spawn "x: 3.0
y: 3.0
theta: 0.0
name: 't1'"

service client

• rosservice info result show the the service implement in /turtlesim package

• most simple service client (without error handler)

#!/usr/bin/env python
import rospy
from turtlesim.srv import Spawn

def main():
    service = rospy.ServiceProxy('/spawn', Spawn)
    service(x=3.0, y=3.0, theta=0.0, name="t1")

if __name__ == "__main__":
    main()

Service client send Request and server answer with response

The service declare by service message that has tow parts request and response

request
---
response

• For example Spawn.srv file under turtlesim.srv folder

float32 x
float32 y
float32 theta
string name
---
string 

When compile the packaged catkin build and install class file that contains the class for example:

• Spawn
• SpawnRequest
• SpawnResponse

#!/usr/bin/env python
import rospy
from turtlesim.srv import Spawn, SpawnRequest

def main():
    service = rospy.ServiceProxy('/spawn', Spawn)
    req = SpawnRequest(x=3.0, y=3.0, theta=0.0, name="t1")
    res = service(req)
    print res.name

if __name__ == "__main__":
    main

ROSPy node templates

Spin and Rate

spin() and rate() are resposible to handle communication events, e.g. arriving messages. If you are subscribing messages, services or actions you must call them to process the events.

Using rospy.rate

• rospy rate

#! /usr/bin/env python

import rospy
from datetime import datetime

NODE_NAME = "reading_laser"

def my_shutdown():
    rospy.loginfo("Shutdown node")

def main():
    rospy.init_node(NODE_NAME)
    rospy.loginfo("Init node: {}".format(NODE_NAME))
    rospy.on_shutdown(my_shutdown)
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        rate.sleep()
        rospy.loginfo(datetime.now().strftime("%b %d %Y %H:%M:%S"))

if __name__ == "__main__":
    main()  

• Loop run every one second (rate 1 Hz)
• Run Node (don't forget to chmod)

[INFO] [1540546445.840602]: Init node: reading_laser
[INFO] [1540546446.842748]: Oct 26 2018 12:34:06
[INFO] [1540546447.842572]: Oct 26 2018 12:34:07
[INFO] [1540546448.842655]: Oct 26 2018 12:34:08

Using rospy.spin

• main thread wait for shutdown (ctrl-c)
• Other event are invoked

#! /usr/bin/env python

import rospy
from datetime import datetime

NODE_NAME = "reading_laser"

def my_shutdown():
    rospy.loginfo("Shutdown node")

def main():
    rospy.init_node(NODE_NAME)
    rospy.loginfo("Init node: {}".format(NODE_NAME))
    rospy.on_shutdown(my_shutdown)
    
    while not rospy.is_shutdown():
        rospy.spin()

if __name__ == "__main__":
    main()  

GTest Hello

GTest

Google test is a framework for writing C++ unit tests

Download and install

• apt (download source code)
• run cmake
• copy to /usr/lib

sudo apt install libgtest-dev
cd /usr/src/gtest
sudo cmake CMakeLists.txt
sudo make
 
# copy or symlink libgtest.a and libgtest_main.a to your /usr/lib folder
sudo cp *.a /usr/lib

Demo Project structure

├── CMakeLists.txt
├── bin
├── build
├── src
│   ├── CMakeLists.txt
│   ├── hello.cpp
│   └── whattotest.cpp
└── tests
    ├── CMakeLists.txt
    ├── tests.cpp
    └── tests_main.cpp

• code under tests (whattotest.cpp)

#include <math.h>

double squareRoot(const double a){
    double b = sqrt(a);
    if (b != b){
        return -1.0;
    }else {
        return sqrt(a);
    }
}

• test main file (tests_main.cpp)

#include <gtest/gtest.h>

int main(int argc, char **argv) {
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

• Tests (tests.cpp)

#include "../src/whattotest.cpp"
#include <gtest/gtest.h>
 
TEST(SquareRootTest, PositiveNos) { 
    ASSERT_EQ(6, squareRoot(36.0));
}
 
TEST(SquareRootTest, NegativeNos) {
    ASSERT_EQ(-1.0, squareRoot(-15.0));
}

• Main CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
project(scope)

#set binary output
set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin)
message(${EXECUTABLE_OUTPUT_PATH})
#set flags
set(CMAKE_CXX_STANDARD 14)

#add source directory
add_subdirectory(src)
add_subdirectory(tests)

• tests folder CMakeLists.txt

# Locate GTest
find_package(GTest REQUIRED)
include_directories(${GTEST_INCLUDE_DIRS})
 
# Link runTests with what we want to test and the GTest and pthread library
add_executable(runTests tests_main.cpp
                        tests.cpp)
target_link_libraries(runTests ${GTEST_LIBRARIES} pthread)

Reference

• Getting started with Google Test (GTest) on Ubuntu
• Googletest Primer
• Advanced googletest Topics
• A quick introduction to the Google C++ Testing Framework

BASH Tips

Gnome Terminal Theme

https://github.com/Mayccoll/Gogh

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

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

Debug

launch.json

• Changed to debug mode
• From combo box -> Add configuration
  • open launch.json
• Set program to debug

"program": "${workspaceFolder}/bin/hello",

Cross-compile strace

Compile strace statically build use on RPi

- Download strace source code strace 4.8
- Download/Pull RPi toolchain: https://github.com/raspberrypi/tools
- Export CC variable, point to compiler RPi toolchain gcc

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

run

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

make

Check

file strace

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

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> follower.py

• 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
│   └── follower.py
├── src
└── worlds
    └── course.world

<?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/course.world"/>
    </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'])
cv2.circle(image, (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

Reference

• Programing robots with ROS book
• OpenCV moments
• My source code

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

CvBridge

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

Demo

• Use as Gazebo camera plugin post as Image source
• Create package named cv_demo run the source code as rosnode cvnode.py 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 cvnode.py

• Termianl 4 (rqt)

rqt

Reference

• Converting between ROS images and OpenCV images

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

Gazebo camera plugin

resource

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

Adding plugin to link element

<gazebo reference="<link_name>">
  <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
    ... 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="libgazebo_ros_camera.so">
        <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
...

tf

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

 <frameName>camera_link</frameName>

Test

• 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="libgazebo_ros_camera.so">
        <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>