Program Analysis

1. File Path

The program file is stored in:

/home/ubuntu/armpi_pro/src/visual_processing/scripts/visual_processing_node.py

(image analysis）

/home/ubuntu/armpi_pro/src/intelligent_grasp/scripts/intelligent_grasp_node.py

(picking control)

2. Program Performance

After the game starts, the robotic arm will rotate back and forth to search for
target. The target will be marked with a bounding box once it is detected. At
this pint, robotic arm will slowly move towards the target, and then grip and
place it at a specific position.
3. Program Analysis

Note: please back up the initial program before making any modifications. It is

prohibited editing the source code files directly to prevent making changes in an

incorrect manner that could lead to robot malfunctions, rendering them irreparable.

3.1 Import Parameter Module

Imported Module Function

The sys module of Python is imported to

import sys access to system-related functionalities and

variables.

The OpenCV library of Python is imported

import cv2 to perform image processing and computer

vision-related functions.

The time module of Python is imported to

import time perform time-related functionalities, such as

delay operations.

The math module of Python is imported to

import math perform mathematical operations and

functions.

The Python library rosy is imported for

import rospy
communication and interaction with ROS.

The NumPy library is imported and is

import numpy as np renamed as np for performing array and

matrix operations.
 The Misc module is imported from

from armpi_pro import Misc arm_pi_pro package to handle the

recognized rectangular data.

The apriltag module is imported from

from armpi_pro import apriltag arm_pi_pro package to perform Apriltag

recognition and processing.

The “RLock” class and “Timer” class is

from threading import RLock, Timer imported from the threading module of

Python for thread-related operations.

All service message types are imported

from std_srvs.srv import * from the std_srvs in ROS for defining and

using standard service messages.

All message types are imported form the

from std_msgs.msg import * std_msgs package in ROS for defining and

using standard messages.

The image message type is imported from

from sensor_msgs.msg import Image the sensor_msgs packages for processing

image data.

The Result message type is imported from

from visual_processing.msg import
the visual_processing package for the
Result
message of image processing results.

The SetParam service type is imported

from visual_processing.srv import
from the visual_processing packages for
SetParam
using customs service related to parameter
 settings.

The Led message type is imported form the

from [Link] import Led [Link] module for controlling or

representing the LED status on a sensor.

All message types are imported from the

chassis_control.msg module, which

from chassis_control.msg import * indicated that all message types defined in

this module is imported to perform the

chassis control.

The SetTarget service type is imported

from visual_patrol.srv import SetTarget from the visual_patrol.srv module is used to

set a target for line following.

The MultiRawIdPosDur message type is

from hiwonder_servo_msgs.msg imported from the

import MultiRawIdPosDur hiwonder_servo_msgs.msg module for

controlling servos.

The PID class is imported from the

from armpi_pro import PID armpi_pro module to perform PID

algorithm.

The bus_servo_control module is imported

from armpi_pro import from the armpi_pro module, including the

bus_servo_control functions and methods related to the servo

control.

from kinematics import ik_transform The ik_transform function is imported from

 the kinematics module to perform

conversion of inverse kinematics.

3.2 Program Logic

The image information is obtained through camera,

By using the camera to capture the visual information, the robotic arm moves
back and forth to locate a colored block. When the block is recognized, the
detected image will be processed to obtain the position information of the
colored block. Then, control the robotic arm to grip the block and place it at a
designated position.
3.3 Code Analysis

From the above diagram flow, the program is mainly used for multiple colors
recognition function and servo control.

3.3.1 Image Processing

Initializing Functions and variables

Binarization

Use the inRange () function from the cv2 library to binarize the image

The first parameter “frame_lab” is the input image.

The second parameter “tuple(color_range['min'])” is the lower limit of threshold.

The third parameter “tuple(color_range['max'])” is the upper lower of threshold.

Dilation and Erosion

To reduce interference and make a smooth image, it is necessary to perform

dilation and erosion operations on the image.

erode() function is applied to erode image. Here uses an example of the code
“eroded = [Link](frame_mask,
[Link](cv2.MORPH_RECT, (2, 2)))” as example. The
meaning of parameters in parentheses are as follow:

The first parameter “frame_mask” is the input image.

The second parameter “[Link](cv2.MORPH_RECT, (2, 2))”

is the structural elements and kernel that determines the nature of operation.
The first parameter in parentheses is the shape of kernel and the second
parameter is the size of kernel.

dilate() function is applied to dilate image. The meaning of parameters in

parentheses is the same as the parameters of erode() function.

Obtain the contour of the maximum area

After processing the above image, obtain the contour of the recognition target.
The findContours() function from the cv2 library is involved in this process.

The erode() function is applied to erode. Here uses an example of code

“contours = [Link](dilated, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)[-2]”.

The first parameter “dilated” is the input image.

The second parameter “cv2.RETR_EXTERNAL” is the contour retrieval mode.

The third parameter “cv2.CHAIN_APPROX_NONE)[-2]” is the approximation

method for contours.

Find the maximum contour from the obtained contours. To avoid interference,
set a minimum value. Only when the area is greater than this minimum value,
the target contour will take effect.

Obtain Position Information

The minAreaRect() function from the cv2 library is used to obtain the minimum
enclosing circle and its center coordinates for the target contour. The circle()
function is employed to display the circumferential in the feedback image.
Determine the color block with the largest area.

3.3.2 Gripping Control

The position of the target on x, y and z axes are obtained after processing
image. Then get the target position calculated by inverse kinematics and grip
it.

The inverse kinematics takes “[Link]((0, round(y_dis + offset_y,

4), -0.08), -180, -180, 0)” as example and the meaning of parameters in
parentheses are as follow:

The first parameter is “(0, round(y_dis + offset_y, 4)”. “0” is the position of the
target on x-axis. “round(y_dis, 4)” is the position of the target on y-axis.
“round(z_dis, 4)” is the position of the target on z-axis.
The second parameter “-180” is the angle of x-axis.

The third parameter “-180” is the range of the pitch angle.

The fourth parameter “0” is the range of pitch angle.

The servo control uses an example of code

“bus_servo_control.set_servos(joints_pub, 20, ( (3, servo_data['servo3']),
(4, servo_data['servo4']), (5, servo_data['servo5']), (6, x_dis)))” and the
meaning of parameters in parentheses is as follow:

The first parameter “joints_pub” is to publish the message of servo control

node.

The second parameter “20” is the running time.

The third parameter is “( (3, servo_data['servo3']), (4, servo_data['servo4']),

(5, servo_data['servo5']), (6, x_dis)”. Among them, “3” is the servo number.
“servo_data['servo3']” and the rest of parameters are the servo angle.