# 11 ROS1-Robotic Arm Control ## 11.1 Robotic Arm Basic Control The robotic arm is an automated mechanical device widely used in the field of robotics technology, with application spanning industrial manufacturing, medical treatment, entertainment services, military operations, semiconductor, manufacturing, and space exploration, among others. Despite their varied forms, they share a common characteristic: the ability to receive commands and accurately position themselves in three-dimensional (or two-dimensional) space to perform tasks. Robotic arms equipped with visual systems enhance their functionality. This document will use the example of JetRover with Mecanum wheel chassis to explain the basic structure and motion control of this robotic arm. The control methods for robotic arms with different types of chassis within the same series are similar. ### 11.1.1 **Get to Know Robotic Arm** JetRover is equipped with a 6DOF robotic arm, composed of intelligent bus servos and metal connection components, which can reach any position in feasible space. Six degrees of freedom consist of X-axis movement, Y-axis movement, Z-axis movement, X-axis rotation, Y-axis rotation, and Z-axis rotation, completing actions such as extension, rotation, and lifting. Additionally, it is equipped with a Dabai DCW depth camera, enabling different functions such as hand tracking, color recognition and sorting, color tracking, line following and obstacle avoidance, waste sorting, navigation and transportation.

The below table illustrates the relevant parameters of the robotic arm: | Name | Instruction | Name | Instruction | | :----------------------- | :------------------------------------- | :---------------------: | :-------------------------------------: | | Servo Composition | 35kg\*6+20kg\*1 High-voltage Bus servo | Servo Accuracy | 0.2° | | Material | Anodized aluminium alloy | Control Method | UART serial port command | | Degree of Freedom | 6DOF + gripper | Communication Baud Rate | 115200 | | Load | (gripping and transporting weight) | Servo Memory | User settings Power-off protection | | Wingspan | 410mm | Read-back Function | Support angle value read-back | | Effective Gripping Range | Radius≤30cm | Servo Protection | Locked protection/ over-heat protection | | Camera Name | Dabai DCW depth camera | Parameter feedback | Temperature, voltage, position | | Pixel | 200W | System | Support windows, Linux and Openwrt | | Resolution | 1920×1080 | Frame Rate | 60FPS | | Connection | USB driver-free | Focusing Method | Auto-focusing | > [!NOTE] > > **Note:** > > - Prior to shipment, our company has completed the debugging of the robotic arm, eliminating the need for users to adjust. > > - This instruction facilitates users in restoring the robotic arm’s status after make modification. > > - The robotic arm is equipped with 6 joints, where the first joint controls the overall horizontal rotation of the robotic arm. During adjustment, it should be ensured that the robotic arm faces forward in relation to the vehicle, avoiding left or right deviation as much as possible. > > - Whether there is deviation in the robotic arm servo can be referenced from the side view and top view diagrams provided in this document. After understanding the structure of the robotic arm, the following content pertains to the Dabai DCW depth camera. Dabai DCW depth camera is designed for service robot scenarios. Based on self-developed ASIC chips, Orbbec developed the Dabai DCW depth camera, a high-performance binocular structured light camera. The depth image resolution of up to 1920×1080@5/10fps, with an average power consumption of less than 1.2W. With just a USB 2.0 interface, the terminal can obtain high-precision 3D depth information for backend utilization, empowering robots to achieve functions such as perception, obstacle avoidance, and navigation.

Its specification parameters are illustrated below:

The specification of the depth camera:

Parameter	Description
Dimension	89.82 ×25.10 ×25.10 mm
Working range	0.2m - 2.5m
Fan	Do not support
Baseline	40mm
Power consumption	Average power consumption <2.3W The maximum power is less than 5.0W (During operation, the duration should be equal to or less than 3ms every 33ms)
Depth FOV	When the camera D2C is not in use, it is H79° V62° D91°±3°. When the camera D2C is in use, it is H79° V55° D88.5°±3°.
Infrared FOV	H79° V62° D91°±3°
Color FOV	16:9： H86° V55° D93.5°±3° 4:3： H64° V55°
Relative accuracy	1.0%(center 81% area) @1000mm 1.1%(center 81% area) @2000mm
Depth resolution @ frame rate	640400@5/10/15/30fps 320200@5/10/15/30fps
Operating system	Support Android and Windows7/10
Data transmission	USB2.0 Type-C
Power Port	Type-C
Application scenario	Indoor
Safety	Class1 laser
Operating temperature	10℃ ~ 40℃

> [!NOTE] > > **Reminder:** There may be individual differences in 3D camera products. The product specifications provided are theoretical values and are for reference only. Please rely on actual performance for accuracy. ### 11.1.2 HTD-35H Serial Bus Servo Instructions HTD-35H bus servo is controlled by serial port commands. The serial port baud rate is 115200. According to the provided communication protocol, user can send corresponding commands to servo to control servo rotation or read servo information. Servo parameters and ID are required to be set before controlling. The interface of this servo is a half-duplex UART asynchronous serial interface so that the signal terminal can send and receive signals. When in use, we can send different commands to different ID through the serial port, which controls servo individually. It is widely applicable in different robotic arm joints.

* **Structure & Specification Parameters** Servo Dimension Diagram:

| Operating voltage | DC 9-12.6V | | ----------------------- | ----------------------------------------------------- | | Rotation speed | 0.18sec/60°（DC 11.1V） | | Rotation torque | 35kg.cm（DC 11.1V） | | Static maximum torque | 35kg.cm（DC 11.1V） | | Servo accuracy | 0.2° | | Angle range | 0-1000 (0°~240°) | | Control method | UART serial command | | Communication baud rate | 115200 | | Memory | Servo settings are automatically saved when power off | | Servo ID number | 0~253 for user setting, ID 1 is default | | Read back Function | Support angle read back function | | Protection | Avoid stalling and overheat | | Parameter feedback | Temperature, voltage and position | | Operating mode | Servo mode and deceleration motor mode | | Gear type | Metal Gear | | Servo Wire | 20cm, other line length can be selected | | Plug-in model | PH2.0-3P | | Weight | 64g | | Size | 54.38mm*20.14mm*45.5mm | | Application | All kinds of bionic robot joints | * **Servo Feature** 1) High voltage servo, more power saving: Compared with the traditional 7.4V servo, the 11.1V high-voltage servo can reduce the current by more than 60%, which greatly improves the battery life of the robot. 2) Serial bus interface: There is an I/O port on the controller board for connecting to the serial bus servo. The servos are connected through the three connectors, which makes the project have clean wiring and beautiful outlook. 3. ID identification and Bus communication Each servo can be set the ID number for the identification. The default ID number for each servo is 1 which is modifiable. The communication method of controller and servo is single-bus communication and its baud rate is 115200. User can set a corresponding ID number to each servo. The command from controller includes ID information so that only the servo matching the ID number can receive the corresponding command completely and then perform actions according to the commands. 4. High-precision potentiometer: The servo uses imported high-precision potentiometer as angle feedback. Excellent precision and linearity of the servo make robot run more table and greatly extend the service life of servo. 5. Strong Torque 35KG strong torque builds up your robot. 6. Position, temperature and voltage feedback: With position, temperature and voltage feedback, you can get the internal data of the servo in real time to protect the servo. 7. Two Operation Modes: Support servo mode and geared motor mode. Under servo mode, servo can rotate to the specific angle within 240 °. Under geared motor mode, servo can rotate within 360° and the direction and speed of rotation can be controlled. 8. Metal Gear: The high-precision inlay of the gears reduces the noise caused by the friction. 9. Metal Shell: Green oxidation metal shell improves heat dissipation ability. * **Installation** Servo horn aims red “**+**” to install, please refer to the following picture.

The interface distribution and instruction refer to the below diagram and table:

| **PIN** | **PIN Instruction** | | :-----: | :----------------------------------------------------------: | | GND | GND | | VIN | Power input | | SIG | Signal terminal, half-duplex UART asynchronous serial interface | ### 11.1.3 PC Software Layout Instruction * **PC Software Instruction** The PC computer corresponds to PLC computer, and is used to send instructions to PLC computer (robot) and receive feedback data from PLC computer. In general, we control PLC computer through running software on PC computer. Only when PC software realizes serial port communication, it can send instructions to and receive feedback data from the robot. Serial port can be considered as USB interface. PC connects to robot through USB interface, and PC software communicate with robot also through USB interface. The introduction to the PC software functions is given below. * **Open PC Software** **1. Open through Robotic Icon** Click Arm icon to open PC software.

**2. Open through Command** 1. Double click

to open terminal. 2. Input the command “**cd software/arm_pc/**” and enter PC software directory. ```py cd software/arm_pc/ ``` 3. Input the command “**python3 main.py**” to open PC software. ```py python3 main.py ``` > [!NOTE] > > Note: Once the related servo nodes are activated (such as mapping and navigation), PC software or servo tool cannot be opened because the serial port is already occupied. **3. PC Software Interface Layout** The interface under “**normal mode**” is as follow.

> [!NOTE] > > **You can switch to “Servo Debug Tool” through the options on the upper left corner. Prior to the shipment, all the robots have been debugged, eliminating the need for the user to debug. If any issues require the use of this tool, it is mandatory to operate under the guidance of our support team, as improper use may result in robotic arm malfunctions.** The interface of the normal mode is divided into below areas:

**(1)：Servo control area** Servo control area displays the icon of the selected servo. You can adjust the servo value by dragging the corresponding slider. | Icon | Icon | | :----------------------------------------------------------: | :--------------------------------------: | |

| ID number of servo | |

| Adjust servo position from 0 to 1000. | |

| Adjust servo deviation from -125 to 125. | **(2)：Action list** The running time and servo data of the current action are displayed on the action list.

| Icon | Function | | :----------------------------------------------------------: | :----------------------------------------------------------: | |

| Action group number | |

| Running time of the action that is time taken to complete this action | |

| Servo value. Double click the figure below

to revise. | **(3)： Action group setting**

Icon	Function
	Action running duration time. Directly click to modify.
	Total running time taking for all the actions in an action group
	If you click this button, joints of robot become loose, and you can drag servos to design any posture
	Read the servo angle you have designed before. This button should be used with
	Add the servo value as a action to the last line of the action list
	Delete action: delete the action selected in the action list Delete all: delete all the action in action list
	Replace the angle value of the action selected in the action list with the servo value in the servo control area. And update the running time as the time set in “Time”
	Insert a new action above the selected action. The running time of this new action is the time set in “Time” and angle value is the current value in servo control area.
	Move the selected action up one line
	Move the selected action down one line
	Click to run all the actions on the action list once (If "Loop" is ticked, JetAuto Pro will repeat the action.)
	Load the data of the saved action group to the action list
	Save the current actions in the action list into the designated path.
	Firstly, open one action group, then click this button, and then open other action group. And these two action groups will be integrated into one.
	Display the saved action groups. You can select the action to run.
	Refresh action group drop-down menu.
	Run the selected action group once.
	Stop running the action group.
	Exit PC software interface

| Click to read the saved servo deviation. | |

| Click to download the adjusted deviation to the robot. | |

| Click to return all the servos to the mid point(500). | ### 11.1.4 Action Calling * **What is “Action Calling”** Action calling is to directly call the edited action group via PC software to let robot perform this action. JetRover has built-in action groups, and its action group files are stored in “**hiwonder/software/arm_pc/ActionGroups**”. You can check and call built-in actions via PC software or command. The specific operation steps are as follow: > [!NOTE] > > Note: Only if action files are saved in “**hiwonder/software/arm_pc/ActionGroups**”, can the files be called

* **Operation Steps** 1. Put down antenna before operation to avoid robot arm of hitting antenna when it is moving. 2. Double click

to enter PC software interface.

3. Click “**Open action file**” button.

4. Select action group you want, then click “**Open**”.

5. Running time of each action and servo values are displayed on the action list

6. You can first select number “1” and click “**Run**” button to run the selected action in action list. If you want to make robot repeat this action group, you can tick “**Loop**” and click “**Run**”.

### 11.1.5 Action Editing * **Introduction** Adjust the angles of corresponding servos based on the target motion to achieve that motion. Multiple actions are combined into an action group. Edit several actions to form a action group so as to make robotic arm pick the block at left. * **Action Realization** **1. Design Action** 1. Put down antenna before operation to avoid robot arm of hitting antenna when it is moving. 2. Double click

to enter PC software interface.

3. Click “**Reset servo**” to make servo return back to mid-point.

4. Drag the slider to set servo values as pictured to make robot arm bend to left.

5. Click “**Add Action**” to add current action to action list.

6. Align robot arm with the block. Adjust value of corresponding servos as follow.

7. Set the time as “**2000ms**”. Click “**Add Action**” to update NO.2 action.

8. Add another transitional action. Set the time as 200ms and click “**Add Action**”.

9. Adjust NO.10 servo to let robot arm pick the block. Set times as 500ms, then click “**Add Action**”

10. Add a transitional action again. Set the time as “200ms”, and click “**Add action**” to form NO.5 action.

11. Adjust servo value to make robot arm pick the block to specific height. Set the time as 2000 ms, and click “**Add action**”

12. After NO.6 action is edited, action group of “**picking block at left**” is complete.

13. Next, let robot arm run the whole action group. Select NO.1 action, then click “**Run**”. If you want to repeat this action, tick “**Loop**” box.

**2. Save Action** > [!NOTE] > > **Note: the name of action group cannot contain “Space”, otherwise the file cannot be saved in later debugging.** In case of future debugging and management, save the edited action group. Click “**Save action file**” and select this path, **hiwonder/software/arm_pc/ActionGroups.** Here takes the name of “**font_pick**” as example, and then click “**Save**” to save the action group.

### 11.1.6 Integrate Action Files * **Introduction** Integrating action files is to integrate two action groups to form a new action group. * **Operation Steps** 1) Put down antenna before operation to avoid robot arm of hitting antenna when it is moving. 2) Double click

to enter PC software interface.

3. Click “**Integrate files**” button, and select the following path.

4. Select and open “**place_left.d6a**” on the pop-up window.

5. This action group is added to the action list.

6) Click “**Integrate action files**” button again. Select and open “**place_right.d6a**”. After that, this action group is added to the end of the first action group.

7) Click NO.1 action, then click “**Run**” to let robot arm perform this integrated action.

8) Click “**Save action files**” button to save this new action group in case of future debugging.

9) Name this new action group, for example “**left_right**”. > [!NOTE] > > **Note: action group name cannot contain “Space”.**

### 11.1.7 Export and Import Action Files * **Introduction** Export the action group files edited on PC software, and import them to other devices of the same type. * **Export Action** This section illustrates the export and import of the "**pick.d6a**" action group file as an example. 1) Click

2) Find “**pick.d6a**” file.

3) Directly drag the action file to the computer desktop to export this file.

* **Import Action** 1) Put down antenna before operation to avoid robot arm of hitting antenna when it is moving. 2) Directly drag “**pick.d6a**” action file to robot system desktop.

3) Then directly drag or copy “**pick.d6a**” file to following path.

4) Double click

to enter PC software interface.

5. Click “**Open action files**” button. Find the file you just import, and open it.

Or directly select the imported action group in this drop-down menu.

6. Click “**Run**” button to let robot arm execute this action group.

## 11.2 Robotic Arm Visual Application ### 11.2.1 Application Overview The robotic arm is a mechanical device that is able to simulate the human arms and is widely used in various fields such as industrial, medical and military. Here are several application scenarios of robotic arms: 1. **Industrial automation**: The application of robotic arms in industrial production is particularly widespread, they can be used for tasks such as handling, assembly, welding, painting, etc., greatly improving production efficiency and quality. 2. **Medical Assistance Therapy:** Robotic arms can be used in operating room to provide doctors with higher precision in surgical procedure, reducing surgical errors and other adverse outcomes. 3. **Electronic Equipment Maintenance**: Robotic arms can be used for electronic equipment maintenance, especially in compact spaces or high-risk environments, where they can prevent personnel from being injured. 4. **Space Exploration**: The application of robotic arms in space exploration missions, such as probing planetary surfaces, collecting samples, and other tasks, is an extremely important technological means. In conclusion, the application scenarios of robotic arms are extremely diverse. With the continuous advancement of technology, the use of robotic arms in various fields will become increasingly widespread. Therefore, this document will introduce the relevant applications of the robotic arm on JetRover, allowing users to experience the different functional effects of visual robotic arms.

The diagram above illustrates the structure of "Robotic Arm Visual Applications" functionality, including hand tracking, color recognition and sorting, color tracking, line-following clearance, waste sorting, navigation and transportation. The following content will be written based on this diagram. ### 11.2.2 Hand Tracking * **Program Logic** What is the application scenario of the hand tracking? 1. Virtual realization hand tracking technology can be used in virtual realization games, enabling players to control game characters' movements, attacks, and other actions through gestures. 2. Medical hand tracking technology can be used in rehabilitation training to help patients regain hand functionality. 3. Educational hand tracking technology can be used in the field of education, allowing students to engage in interactive learning through gestures. 4. Smart home hand tracking technology can be used in smart homes, allowing users to control home devices' switches, adjustments, and other operations through gestures. 5. Industrial production hand tracking technology can be used in industrial production, allowing workers to control robots' operations through gestures, thus improving production efficiency. The hand features detection in JetRover utilizes MediaPipe, an open-source multimedia machine learning model application framework. It can run cross-platform on mobile devices, workstations, and servers, and supports mobile GPU acceleration. It also supports the inference engines of TensorFlow and TF Lite, allowing any TensorFlow and TF Lite models to be used with MediaPipe. Additionally, on mobile and embedded platforms, MediaPipe also supports device-native GPU acceleration. Firstly, it is necessary to build a hand recognition model and subscribe to the topic messages published by the camera node to obtain images. Then, process the images, such as flipping, and detect hand information within the images. Next, based on the lines connecting the keypoints of the hand, obtain the position of the center point of the hand. Finally, control the robotic arm to follow the up-and-down movement of the hand's center point. The source code of the program is located in: **/home/hiwonder/ros_ws/src/hiwonder_example/scripts/hand_track/hand_track_node.py** * **Operation Steps** > [!NOTE] > > **Note:** **The entered command should be case sensitive and “Tab” key can be used to complement the key words.** 1. Start JetRover and connect it to NoMachine. 2. Double click on

* **Program Analysis** **The source code of the program is stored in** **/home/ros_ws/src/hiwonder_example/scripts/color_sorting/color_sorting_node.py** **1. Subscribe to the Node** Subscribe to the topic messages “**/color_detect/color_inf**” and “**/color_detect/image_result/**” from the color recognition node to obtain the recognized color information and images. ```py rospy.Subscriber('/color_detect/color_info', ColorsInfo, self.get_color_callback) rospy.Subscriber('/color_detect/image_result/', Image, self.image_callback) ``` **2. Initialize Robotic Arm Posture** Through calling the “**pick_init**” action group file, prepare the robotic arm for the sorting task. ```py self.controller.runAction('pick_init') ``` **3. Enable Sorting with Multiple Threads** Create a new thread,call the “**pick**” function, and execute the sorting task. ```py threading.Thread(target=self.pick, daemon=True).start() ``` **4. Draw Recognition Area Frame** By using the rectangle function, draw a box around the recognition area. ```py cv2.rectangle(self.image, (self.pick_roi[2] - 25, self.pick_roi[0] - 25), (self.pick_roi[3] + 25, self.pick_roi[1] + 25), (0, 255, 255), 2) ``` The specific meanings of the function parameters are as follows: The first parameter "**self.image**" is the image on which to draw. The second parameter "**(self.pick_roi\[2\] - 25, self.pick_roi\[0\] - 25)**" is the coordinate of the upper left corner of the rectangle. The third parameter "**(self.pick_roi\[3\] + 25, self.pick_roi\[1\] + 25)**" is the coordinate of the lower right corner of the rectangle. The fourth parameter "**(0, 255, 255)**" is the color of the rectangle. The fifth parameter "**2**" is the thickness of the rectangle's lines. **5. Execute Sorting Action** Based on the matching results of the colors, call the corresponding action group to complete the sorting task. ```py if self.target_color == 'red': self.controller.runAction('pick') self.controller.runAction('place_center') elif self.target_color == 'green': self.controller.runAction('pick') self.controller.runAction('place_left') elif self.target_color == 'blue': self.controller.runAction('pick') self.controller.runAction('place_right') self.controller.runAction('pick_init') ``` * **Gripping Calibration** The default recognition and gripping area of the program is located in the middle. In normal circumstances, no adjustment is necessary. However, due to differences in camera parameters, there may be situations where the robot arm cannot grip the blocks. This can be addressed by adjusting the position of this area through program instructions. The specific steps are as follow: 1) Start JetRover and connect it to NoMachine. 2. Double click on

6. Run the program according to “**[11.2.3 Color Recognition and Sorting->Operation Steps]()**”. ### 11.2.4 Color Tracking * **Program Logic** The first-person view is the perspective of the robot itself. In this game, robot will take the first-person view to complete the color tracking task. Before starting the game, prepare yourself the required colored blocks. First of all, subscribe to the topic messages published by color recognition node to obtain the color information. Subsequently, after matching the target color, obtain the center of the target image. Finally, by using inverse kinematics, calculate the required angle to align the center position of the screen with the center of the target image. Publish the corresponding topic message, control the servo motion and make the robotic arm follow the movement of the target. The source code of the program is stored in: **/home/ros_ws/src/hiwonder_example/scripts/color_track/color_track_node.py** ```py class ColorTrackNode: def __init__(self, name): rospy.init_node(name) self.z_dis = 0.36 self.y_dis = 500 self.x_init = transform.link3 + transform.tool_link self.center = None self.running = True self.start = False self.name = name self.pid_z = pid.PID(0.00008, 0.0, 0.0) self.pid_y = pid.PID(0.05, 0.0, 0.0) signal.signal(signal.SIGINT, self.shutdown) ``` * **Operation Steps** > [!NOTE] > > **Note: the input command should be case sensitive, and the “Tab” key can be used to complement the key words.** 1) Start JetRover and connect it to NoMachine. 2. Double click on

* **Program Analysis** **1. Subscribe and Publish Nodes** Publish topic message “**servo_controllers/port_id_1/multi_id_pos_dur**” to control the rotation of the robotic arm servos; publish the topic messages “**/hiwonder_controller/cmd_vel**” to control the motion of the chassis; subscribe to topic messages “**/color_detect/color_info**” for color recognition to obtain the recognized color information. ```py self.joints_pub = rospy.Publisher('/servo_controllers/port_id_1/multi_id_pos_dur', MultiRawIdPosDur, queue_size=1) # 舵机控制 self.mecanum_pub = rospy.Publisher('/hiwonder_controller/cmd_vel', Twist, queue_size=1) # 底盘控制 rospy.Subscriber('/color_detect/color_info', ColorsInfo, self.get_color_callback) ``` **2. Set Target Color** Set the color to be recognized and tracked. ```py msg_red = ColorDetect() msg_red.color_name = msg.data msg_red.detect_type = 'circle' res = rospy.ServiceProxy('/color_detect/set_param', SetColorDetectParam)([msg_red]) ``` **3. Obtain the Center Position of the Target Image** Based on the available color recognition information, determine the center position data of the target image. ```py if msg.data != []: if msg.data[0].radius > 10: self.center = msg.data[0] else: self.center = None else: self.center = None ``` **4. PID Data Update** The PID algorithm is used to update the coordinates of the Y-axis and Z-axis. ```py self.pid_y.SetPoint = self.center.width/2 self.pid_y.update(self.center.x) self.y_dis += self.pid_y.output if self.y_dis < 200: self.y_dis = 200 if self.y_dis > 800: self.y_dis = 800 self.pid_z.SetPoint = self.center.height/2 self.pid_z.update(self.center.y) self.z_dis += self.pid_z.output if self.z_dis > 0.46: self.z_dis = 0.46 if self.z_dis < 0.36: self.z_dis = 0.36 ``` The height and width of the image are divided by 2 to obtain the coordinates of the center position of the image. Then compare it with the center of the hand to obtain the deviation and update the PID data. Finally, use the output result update the coordinates of X-axis and Z-axis. **5. Robotic Arm Moves Based on the Calculated Angles** Based on the above calculated angles, set the rotation angle for each servo. Then publish the topic to control the movement of the robotic arm. ```py if res[1]: servo_data = res[1] set_servos(self.joints_pub, 0.02, ((10, 500), (5, 500), (4, servo_data[3]), (3, servo_data[2]), (2, servo_data[1]), (1, self.y_dis))) ``` * **Extension Function** The program defaults to recognize red. However, you can change the recognition color to green or blue through modifying the codes in corresponding program. In this section, the default recognition color is changed to green as example. The specific operation steps area as follow: 1) Start JetRover and connect it to NoMachine. 2. Double click on

to open the command line terminal. 3. Input the command “**roscd hiwonder_example/scripts/color_track/**” and press Enter to access the program directory. ```py roscd hiwonder_example/scripts/color_track/ ``` 4. Input the command “**roscd hiwonder_example/scripts/color_track/”** to open the program file. ```py roscd hiwonder_example/scripts/color_track/ ``` ```py vim color_track_node.py ``` 5. Press “i” key to enter edit mode and modify the assignment of the “**msg_red.color_name**” parameter to “**green**”.

6. After the modification is completed, enter “**:wq**” to save and exit the program file.

7. Operate the game based on “**[11.2.4 Color Tracking->Operation Steps]()**”. ### 11.2.5 Line-Following Obstacle Clearance * **Program Logic** During JetRover moves forward along the black line, it will automatically clear the obstacles around the black line. Before the game starts, it's necessary to affix the black line in advance and place JetRover in front of the black line. Ensure that there are no other objects of the same color around to prevent interference with recognition, and place the obstacle blocks along the black line. Firstly, subscribe to the topic messages published by the color recognition node and Lidar node to obtain the recognition color information, captured image data, and Lidar data. Next, obtain the coordinates of the center position of the line within the image. Calculate the deviation from the center position of the image, update the PID data, and correct the robot’s driving direction. Finally, when obstacles are detected on the line, call the obstacle-clearing action group and remove the block obstacles. The source code of this program is stored in **/home/ros_ws/src/hiwonder_example/scripts/line_follow_clean/line_follow_clean_node.py** ```py class LineFollowCleanNode: def __init__(self, name): rospy.init_node(name) self.running = True self.image = None self.count = 0 self.count_stop = 0 self.stop = False self.line_color = 'black' self.line_x = None self.temp_line_x = None self.object_blue = 'blue' self.object_red = 'red' self.object_green = 'green' self.center = None self.temp_center = None self.stop_threshold = 0.4 self.scan_angle = math.radians(90) # radians self.pid = pid.PID(0.008, 0.0, 0.0) self.pid_x = pid.PID(0.001, 0.0, 0.0) pick_roi = rospy.get_param('~roi') self.pick_roi = [pick_roi['y_min'], pick_roi['y_max'], pick_roi['x_min'], pick_roi['x_max']] #[y_min, y_max, x_min, x_max] self.start_pick = False ``` * **Operation Steps** > [!NOTE] > > **Note: the input command should be case sensitive, and the “Tab” key can be used to complement the key words.** 1) Start JetRover and connect it to NoMachine. 2. Double click on

to open the command line terminal. 3. Input the command “**sudo systemctl stop start_app_node.service**” and press Enter to disable the app service. ```py sudo systemctl stop start_app_node.service ``` 4. Input the command “**roslaunch hiwonder_example line_follow_clean_node.launch**” to start the game. ```py roslaunch hiwonder_example line_follow_clean_node.launch ``` 5. If need to terminate the game, please press “**Ctrl+C**” in the terminal. If fail to do so, please try multiple times. * **Outcome** After the game starts, JetRover moves alone the recognized black line. When it encounters the colored block obstacles along the way, it will pause, grip the obstacle and place it on the left side. Afterward, it will continue moving forward. * **Program Analysis** **1. Subscribe and Publish Node Topics** Subscribe to the topic messages “**/color_detect/color_info**” and “**/color_detect/image_result/**” from color recognition nodes to obtain the recognized color information and the captured image. Subscribe to the topic message “**/scan**” from Lidar node to control the motion of the chassis. Publish topic message “**/hiwonder_controller/cmd_vel**” from the chassis control node to control the motion of the chassis. ```py self.lidar_type = os.environ.get('LIDAR_TYPE') self.lidar_sub = rospy.Subscriber('/scan', LaserScan, self.lidar_callback) # 订阅雷达 self.mecanum_pub = rospy.Publisher('/hiwonder_controller/cmd_vel', Twist, queue_size=1) # 底盘控制 rospy.Subscriber('/color_detect/color_info', ColorsInfo, self.get_color_callback) rospy.Subscriber('/color_detect/image_result/', Image, self.image_callback) ``` **2. Initialize Robotic Arm Altitude** Invoking the “**invoking**” action group file to make the robotic arm present the line-following posture. ```py self.controller.runAction('line_follow_init') ``` **3. Enable multi-threading for obstacle clearance** Create a thread and invoke the “**pick**” function to execute the obstacle clearance task. ```py threading.Thread(target=self.pick, daemon=True).start() ``` **4. PID Calibration** After obtaining the center position of the image, update the data for the linear velocity X and angular velocity Z in PID, and restrict the range using the `set_range` function. ```py self.pid_x.SetPoint = (self.pick_roi[1] + self.pick_roi[0])/2 self.pid_x.update(self.center.y) self.pid.SetPoint = (self.pick_roi[2] + self.pick_roi[3])/2 self.pid.update(self.center.x) twist.linear.x = misc.set_range(self.pid_x.output, -0.1, 0.1) twist.angular.z = misc.set_range(self.pid.output, -0.5, 0.5) ``` Take an example of “**misc.set_range(self.pid_x.output, -0.08, 0.08)**”. The meaning of the parameters in the function is as follow: The first parameter “**self.pid_x.output**” is the input linear velocity X output data, measured in meters per second. The second parameter “**-0.08**” is the minimum value to which the data is constrained. The third parameter "**0.08**" is the maximum value to which the data is constrained; if the data exceeds this maximum value, the output will be set to the maximum value. **5. Publish Mecanum Wheels Driving Topic** Publish the updated data through the underlying control node **self.mecanum_pub**. ```py if not self.stop: self.mecanum_pub.publish(twist) else: self.mecanum_pub.publish(Twist()) ``` **6. Draw a bounding box for the recognized area.** Through using the rectangle function, draw a bounding box for the recognized area. ```py if not self.start_pick and not self.debug: cv2.rectangle(self.image, (self.pick_roi[2] - 30, self.pick_roi[0] - 30), (self.pick_roi[3] + 30, self.pick_roi[1] + 30), (0, 255, 255), 2) cv2.imshow('image', self.image) ``` The meaning of the parameters are as follow: The first parameter “**self.image**” is the image on which the rectangle is drawn. The second parameter "**(self.pick_roi\[2\] - 30, self.pick_roi\[0\] - 30)**" is the coordinate of the top-left corner of the rectangle. The third parameter "**(self.pick_roi\[3\] + 30, self.pick_roi\[1\] + 30)**" is the coordinate of the bottom-right corner of the rectangle. The fourth parameter "**(0, 255, 255)**" is the color of the rectangle. The fifth parameter "**2**" is the thickness of the rectangle's lines. **7. Perform Obstacle Clearing Action** After recognizing the target obstacle, stop the robot and call the `move_object` action group file to achieve obstacle clearing task. ```py if self.start_pick: self.stop_srv_callback(Trigger()) self.mecanum_pub.publish(Twist()) rospy.sleep(0.5) self.controller.runAction('move_object') self.controller.runAction('line_follow_init') rospy.sleep(0.5) self.start_pick = False self.start_srv_callback(Trigger()) else: rospy.sleep(0.01) ``` * **Gripping Adjustment** In the program, the recognition and gripping area are located in the middle of the image by default, no need for adjustment. However, due to the discrepancy in camera parameters, there might be cases where the robot arm cannot grip the color black. In such situations, you can adjust the position of this area using commands. Here are the specific steps: 1) Start JetRover and connect it to NoMachine. 2. Double click on

6) Finally, run the program according to “**[11.2.5 Line-Following Obstacle Clearance->Operation Steps]()**”. ### 11.2.6 Waste Sorting * **Program Logic** Waste sorting involves the robot recognizing waste cards in front of the camera, and transporting them to the fixed waste card classification areas. Before the game, prepare the waste cards. You can find the image collection of the waste cards under the same directory and print them out. First, subscribe to the topic massage published by the YOLOv5 target detection node to obtain the recognized card information and the card images. Next, match the obtained card information to find out the corresponding waste classification. Finally, based on the waste classification, execute the corresponding sorting action group to complete the task. The source code of the program is stored in **/home/ros_ws/src/hiwonder_example/scripts/garbage_classification/garbage_classification.py** * **Operation Steps** > [!NOTE] > > **Note: the input command should be case sensitive, and the “Tab” key can be used to complement the key words.** 1) Start JetRover and connect it to NoMachine. 2) Double click on

to open the command line terminal. 3) Input the command “**sudo systemctl stop start_app_node.service**” and press Enter to disable the app service. ```py sudo systemctl stop start_app_node.service ``` 4. Input the command “**roslaunch hiwonder_example garbage_classification.launch**” to enable the target detection service. ```py roslaunch hiwonder_example garbage_classification.launch ``` When the below information appears, it indicates that the servo is successfully enabled.

6) If you want to terminate the game, please press “**Ctrl+C**” in the terminal. If fail to do so, please try multiple times. * **Outcome** After the game starts, JetRover recognizes the waste card within the image. Then place the waste card within the yellow box on the image, the robotic arm will grip the card and transport it to the respective waste classification area. * **Program Analysis** **1. Create waste category** Create the corresponding waste categories for each waste card. ```py WASTE_CLASSES = { 'food_waste': ('BananaPeel', 'BrokenBones', 'Ketchup'), 'hazardous_waste': ('Marker', 'OralLiquidBottle', 'StorageBattery'), 'recyclable_waste': ('PlasticBottle', 'Toothbrush', 'Umbrella'), 'residual_waste': ('Plate', 'CigaretteEnd', 'DisposableChopsticks'), } ``` **2. Subscribe to Node Topic** Subscribe to the topics “**/yolov5/object_image**” and “**/yolov5/object_detect**” from the YOLOv5 target detection node to obtain the recognized card information and the images. ```py rospy.Subscriber('/yolov5/object_image', Image, self.image_callback) # 摄像头订阅 rospy.Subscriber('/yolov5/object_detect', ObjectsInfo, self.get_object_callback) ``` **3. Initialize Robotic Arm Posture** Invoke the “garbage_pick_init” action group file to put the robotic arm into the waste classification pose. ```py rospy.sleep(5) self.controller.runAction('garbage_pick_init') rospy.sleep(2) ``` **4. Enable multi-threading for Waste Sorting** Create a thread and invoke the “**pick**” function to execute the waste classification task. ```py threading.Thread(target=self.pick, daemon=True).start() ``` **5. Draw a bounding box for the recognized area.** Through using the rectangle function, draw a bounding box for the recognized area. ```py cv2.rectangle(self.image, (self.center[0] - 45, self.center[1] - 45), (self.center[0] + 45, self.center[1] + 45), (0, 0, 255), 2) ``` The meaning of the parameters are as follow: The first parameter “**self.image**” is the image on which the rectangle is drawn. The second parame**ter “(self.pick_roi\[2\] - 45, self.pick_roi\[0\] - 45)"** is the coordinate of the top-left corner of the rectangle. The th**ird parameter "(self.pick_roi\[3\] + 45, self.pick_roi\[1\] + 45)"** is the coordinate of the bottom-right corner of the rectangle. The fourth parameter "**(0, 255, 255)**" is the color of the rectangle. The fifth parameter "**2**" is the thickness of the rectangle's lines. **6. Print Classification Information** According to the recognized card information, traverse the waste classification dictionary, and print out the matching waste classification. ```py for k, v in WASTE_CLASSES.items(): if self.current_class_name in v: waste_category = k break self.class_name = None print(waste_category) ``` **7. Execute Waste Sorting Action** Invoke the action group file “**garbage_pick”** to grip the waste card. Then invoke the corresponding action group file to transport this card to the respective waste card classification area. ```py self.controller.runAction('garbage_pick') if waste_category == 'food_waste': self.play('food_waste') self.controller.runAction('place_food_waste') elif waste_category == 'hazardous_waste': self.play('hazardous_waste') self.controller.runAction('place_hazardous_waste') elif waste_category == 'recyclable_waste': self.play('recyclable_waste') self.controller.runAction('place_recyclable_waste') elif waste_category == 'residual_waste': self.play('residual_waste') self.controller.runAction('place_residual_waste') self.controller.runAction('garbage_pick_init') ``` ### 11.2.7 Fixed Point Navigation * **Program Logic** First, subscribe to the topics published by the camera node to obtain images. Next, activate the navigation service to receive the location information of the destination. Finally, upon reaching the destination and detecting the target block, the servo control node publishes topic messages to command the robotic arm to complete the gripping and transporting tasks. * **Operation Steps** > [!NOTE] > > **Note: the input command should be case sensitive, and the “Tab” key can be used to complement the key words.** 1. Before starting the game, it’s necessary to complete the mapping task in the field of the navigation and transportation, prepare the colored blocks and mark the placement position in red within the area. 2. Start JetRover and connect it to NoMachine. 3. Input the command “**sudo systemctl stop start_app_node.service**” and press Enter to disable the app service. ```py sudo systemctl stop start_app_node.service ``` 4. Input the command “**roslaunch hiwonder_example navigation_transport.launch map:=map_01**” to start the game. > [!NOTE] > > Note: by default, after gripping the colored block, the robot will directly place ti down upon reaching the next target point. If you need to place it at a specific target location, you can add “**place_without_color:=false**” to the end of the command. ```py roslaunch hiwonder_example navigation_transport.launch map:=map_01 ```

7) If you want to terminate the game, please press “**Ctrl+C**” in the terminal. If fail to do so, please try multiple times. * **Outcome** After opening RVIZ, you need to first check if the position of the robot on the map aligns with its actual potion. If they do not align, manual adjustment may be required. You can utilize the "**2D Pose Estimate**" tool in RVIZ to perform this adjustment. There are three tools in the menu bar, including 2D Pose Estimate, 2D Nav Goal and Publish Point.

“**2D Pose Estimate**” is used to set the initial position of JetRover, “**2D Nav Goal**” is used to set a target point and “**Publish Point**” is used to set multiple target points. Click “**2D Nav Goal**” in the menu bar, and select one point by clicking the mouse as the target destination. After the point is set, JetRover will automatically generate the route and move toward the point.

After navigating to the location with the blue block, the robot will automatically grasp the block upon recognition. Then, it will navigate to the placement area with the red mark. Upon arrival, the robot will automatically place the block, completing the transportation task. > [!NOTE] > > **Note: when the program starts, it can merely complete the entire process once. If you need to perform another gripping and placing cycle, you will need to restart the game.** * **Program Analysis** The source code of this program is located in **/home/ros_ws/src/hiwonder_example/scripts/navigation_transport/navigation_transport.py** ```py def start_pick_srv_callback(self, msg): global markerArray rospy.loginfo("start navigaiton pick") markerArray = MarkerArray() marker_Array = MarkerArray() marker = Marker() marker.header.frame_id = map_frame marker.action = Marker.DELETEALL marker_Array.markers.append(marker) mark_pub.publish(marker_Array) markerArray = MarkerArray() pose = PoseStamped() pose.header.frame_id = map_frame pose.header.stamp = rospy.Time.now() data = msg.data q = common.rpy2qua(math.radians(data.roll), math.radians(data.pitch), math.radians(data.yaw)) pose.pose.position.x = data.x pose.pose.position.y = data.y pose.pose.orientation = q ``` **1. Subscribe to Node Topics** Subscribe to topics messages of camera node and navigation node to obtain image data and navigation position information. ```py map_frame = rospy.get_param('~map_frame', 'map') nav_goal = rospy.get_param('~nav_goal', '/nav_goal') move_base_result = rospy.get_param('~move_base_result', '/move_base/result') ``` **2. Enable multi-threading for transportation** Build a thread and invoke the “**pick_thread**” function to execute the transportation. ```py threading.Thread(target=action_thread, daemon=True).start() ``` **3. Identity Target** Label the central position of the target block using the **circle** function. ```py cv2.circle(img, (center_x, center_y), 5, (0, 255, 255), -1) # 画出中心点 ``` The specific meanings of the function parameters are as follows: The first parameter "**img**" is the image. The second parameter "**(center_x, center_y)**" is the coordinate of the center of the circle. The third parameter "**5**" is the radius of the circle. The fourth parameter "**(0, 255, 255)**" is the color of the circle. The fifth parameter "**-1**" is the size of the line to draw, with negative numbers indicating a solid circle. **4. PID Calibration** After obtaining the center position of the color block image, calculate the deviation from the center position of the screen, update the data of PID's linear velocity and angular velocity, and adjust the position between the robot and the color block. ```py # 以图像的中心点的x，y坐标作为设定的值，以当前x，y坐标作为输入# linear_pid.SetPoint = pick_stop_y if abs(object_center_y - pick_stop_y) <= d_y: object_center_y = pick_stop_y if status != "align": linear_pid.update(object_center_y) # 更新pid tmp = linear_base_speed + linear_pid.output ``` * **Gripping Calibration** The default recognition and gripping area of the program is located in the center of the image. No adjustments are required for the normal circumstance. If the robotic arm fails to grip the colored blocks during the game, you can adjust the position of this area through the program command. The specific steps are as follow: 1. Start JetRove and connect it to Nomachine remote control software. 2. Click on

to open the command line terminal. Input the command “**roslaunch hiwonder_example automatic_pick.launch debug:=ture**”. ```py roslaunch hiwonder_example automatic_pick.launch debug:=ture ``` 3. After the robotic arm reaches the gripping position, place the colored block at the center of the gripper and wait for the robotic arm to reset before griping again, indicating the calibration is completed. Upon the completion of the calibration, the terminal will print the pixel coordinates of the colored block in the image and the “**print finish**” prompt message.

“**pick_stop_pixel_coordinate**” refers to the pixel coordinates of the gripping position in the image. The first parameter represents the x-axis coordinate. Decreasing this value shifts the horizontal position to the left, while increasing it shifts the gripping horizontal position to the right. The second parameter represents the y-axis coordinate. Decreasing this value moves the gripping position closer, while increasing it moves the gripping position farther away. Generally, you can rely on automatic calibration results, but you can also adjust it according to your personal preference. “**place_stop_pixel_coordinate"** refers the pixel coordinates of the placement position in the image. The first parameter represents the x-axis coordinate. Increasing this value shifts the placement position to the left, while decreasing it shifts the placement position to the right. The second parameter represents the y-axis coordinate. Decreasing this value adjusts the placement position closer, while increasing it moves the gripping position farther away. > [!NOTE] > > Note: Automatic calibration solely calibrates the coordinates of the gripping position. The coordinates of the placement position are not automatically calibrated. If a placement target is set and the placement effect is not satisfactory, manual adjustment is required.

4\) Upon the completion of the modification, please start the game according to “**[11.2.7 Fixed Point Navigation->Operation Steps]()**”. ## 11.3 3D Vision ### 11.3.1 Edge Detection * **Overview** When the robot moves, it may encounter various situations where the road has steps or the sunken ground. If there is no corresponding detection and handling measures, the robot takes the risk of falling. When the route is relatively flat and smooth, the ranging distance remains relatively consistent within two different areas, the distance change is relatively continuous. However, when there are steps or depressions ahead, there is a significant discontinuity in the ranging distance values. Therefore, during the autonomous navigation, it is of great importance to ensure the safety of the robot’s movement. To tackle this issue, it is possible to assess the safety of the robot's forward direction by utilizing depth information obtained through depth camera. * **Operation Steps** > [!NOTE] > > **Note: the entered command should be case sensitive, and the “Tab” key can used to complemented the key words.** 1. Start JetRover and connect it to Nomachine remote control system. Regarding the Remote desktop tool installation and connection, please refer to “**[Appendix->Remote Desktop Software]()**.” 2. Double click on

The program of the edge detection will be analyzed in this section. The path to the source code of the program is as follow: **~/ros_ws/src/hiwonder_example/scripts/rgbd_function/prevent_falling_node.py** **1. Import Module** ```py #!/usr/bin/env python3 # encoding: utf-8 # @data:2023/12/18 # @author:aiden # 防跌落 import os import cv2 import math import rospy import queue import signal import numpy as np from hiwonder_sdk import common from sensor_msgs.msg import Image from std_srvs.srv import SetBool from geometry_msgs.msg import Twist from hiwonder_servo_msgs.msg import MultiRawIdPosDur from hiwonder_servo_controllers import bus_servo_control ``` From the above image, import the module. Firstly, the required module is imported into the code, including OpenCV, ROS, NumPy, etc. 1. The realization of edge detection ```py class PreventFallingNode(): ``` 2. Initialize function： Perform some basic settings in the initialization function of the class, including initializing ROS node, setting signal handling functions, and configuring some parameter settings. ```py rospy.init_node(name, anonymous=True) signal.signal(signal.SIGINT, self.shutdown) self.running = True self.turn = False self.plane_high = rospy.get_param('~plane_high', 0.34) self.time_stamp = rospy.get_time() self.image_queue = queue.Queue(maxsize=2) self.left_roi = [290, 300, 95, 105] self.center_roi = [290, 300, 315, 325] self.right_roi = [290, 300, 535, 545] ``` **self.plane_high = rospy.get_param('~plane_high', 0.34)**: The code **“self.plane_high = rospy.get_param('~plane_high', 0.34)”** retrieves the parameter named “**~plane_high**” from ROS parameter server. If this parameter is none, it defaults to the value 0.34. The parameter “plane_high” represents the height of the current plane or region. The code “**self.time_stamp = rospy.get_time(): the line of card**” obtains the current timestamp and store it in the **time_stamp** member variable. It is used to record the time when the event occurs. The code“**self.image_queue = queue.Queue(maxsize=2)**” is applied to create a queue for the storage of the image data. The maximum size of the queue is set to 2, indicating it can store at most two elements.Queues are commonly used for data synchronization in multi-threading or asynchronous programming. The code “**ROI（Region of Interest）**” is used to define three ROI, corresponding to left, center and right regions respectively. These ROI will be used for the subsequent image processing. This line of code “**self.left_roi = \[290, 300, 95, 105\]**” defines a member variable named **left_roi**, which is a list containing four integers. These values represent the coordinates of the Region of Interest (ROI) in image processing, such as the coordinates of the top-left and bottom-right corners. Similarly, this line of code “**self.center_roi = \[290, 300, 315,** 325\]” defines center_roi, which also represents a region of interest in image processing. This line of code “**self.right_roi = \[290, 300, 535, 545\]**” defines right_roi, which similarly represents a region of interest in the image. 3. Image queue: a queue is used to store the most recent two frames of depth images. ```py def depth_callback(self, ros_depth_image): depth_image = np.ndarray(shape=(ros_depth_image.height, ros_depth_image.width), dtype=np.uint16, buffer=ros_depth_image.data) if self.image_queue.full(): # 如果队列已满，丢弃最旧的图像 self.image_queue.get() # 将图像放入队列 self.image_queue.put(depth_image) ``` **def depth_callback(self, ros_depth_image):** Difine a method named **depth_callback**, which takes one parameter: **ros_depth_image**. This parameter is a ROS image message containing depth image data. **depth_image = np.ndarray(...):** Convert the ROS image message to a NumPy array, facilitating the subsequent image processing. “**shape=(ros_depth_image.height, ros_depth_image.width)**” specifies the shape of the array, which is the height and width of the image. “**dtype=np.uint16**” specifies the data type of the array as unsigned 16-bit integer, matching the data type of dept image. “**buffer=ros_depth_image.data**” specifies the buffer of the data, which is the data extracted from the ROS image message. **if self.image_queue.full():** Check if the image queue is full. If the queue is full, the next line of code will be executed. **self.image_queue.get():** If the queue is full, the oldest image will be removed from this queue, ensuring that only two images are contained in the queue. **self.image_queue.put(depth_image):** Put the converted depth image into the queue. If the queue is not full, this line of code will be executed. By using a queue, it ensures that old images are not lost when processing new images, and they can be processed in the order they are received. 4. Publisher: Two publishers are created, one for controlling motors and the other for controlling the robot’s velocity. ```py servos_pub = rospy.Publisher('/servo_controllers/port_id_1/multi_id_pos_dur', MultiRawIdPosDur, queue_size=1) self.mecanum_pub = rospy.Publisher('/hiwonder_controller/cmd_vel', Twist, queue_size=1) ``` 5. Setting up the depth camera: invoking a service to configure the depth camera and publish a default velocity message. ```py rospy.wait_for_service('/depth_cam/set_ldp') rospy.ServiceProxy('/depth_cam/set_ldp', SetBool)(False) ``` 6. Dept image callback function： ```py rospy.Subscriber('/depth_cam/depth/image_raw', Image, self.depth_callback) ``` When receiving a new depth image from the depth camera, this callback function is triggered. It first converts the ROS message to Numpy array, and then puts the array into the image queue. If the queue is full, the oldest image will be discarded. 7. Run the `self.run()` function: ```py def run(self): ``` At the end of the class, the **run()** function is called, but it is not defined in this code. This code snippet is incomplete, or the **run()** function is defined elsewhere. 8. Function to obtain depth distance information within the ROI range. ```py def get_roi_distance(self, depth_image, roi): roi_image = depth_image[roi[0]:roi[1], roi[2]:roi[3]] try: distance = round(float(np.mean(roi_image[np.logical_and(roi_image>0, roi_image<30000)])/1000), 3) except: distance = 0 return distance ``` **def get_roi_distance(self, depth_image, roi):** The code “**def get_roi_distance(self, depth_image, roi)**” defines a method named `get_roi_distance`, which receives three parameters including self (representing the class instance itself), depth_image (representing the input depth image) and roi (representing the four-tuple of ROI, i.e, y1, y2, x1, x2\]) **roi_image = depth_image\[roi\[0\]:roi\[1\], roi\[2\]:roi\[3\]\]** The code “**roi_image = depth_image\[roi\[0\]:roi\[1\], roi\[2\]:roi\[3\]\]**” uses slicing to extract the region of interest from “depth_image” and assigns it to “roi_image”. **distance = round(float(np.mean(roi_image\[np.logical_and(roi_image\>0, roi_image\<30000)\])/1000), 3)** The code “**distance = round(float(np.mean(roi_image\[np.logical_and(roi_image\>0, roi_image\<30000)\])/1000), 3)**” first filters out the pixel values in roi_image that are greater than 0 and less than 30000. Then it calculates the average value of these pixel values and converts it to a floating-point number. Next, it divides the average value by 1000 to convert the units to meters. Finally, it uses the round function to round the result to three decimal places and assigns the result to distance. **distance = 0：** If the exception occurs, set distance as 0. **return distance：** Return the calculated distance value. 9. Robot steering decision-making: ```py def move_policy(self, left_distance, center_distance, right_distance): if abs(left_distance - self.plane_high) > 0.02 or abs(center_distance - self.plane_high) > 0.02 or abs(right_distance - self.plane_high) > 0.02: twist = Twist() twist.angular.z = 0.8 self.turn = True self.time_stamp = rospy.get_time() + 0.3 self.mecanum_pub.publish(twist) else: if self.turn: self.current_time_stamp = rospy.get_time() if self.time_stamp < self.current_time_stamp: self.turn = False self.mecanum_pub.publish(Twist()) self.time_stamp = rospy.get_time() + 0.2 else: self.current_time_stamp = rospy.get_time() if self.time_stamp < self.current_time_stamp: twist = Twist() twist.linear.x = 0.2 self.mecanum_pub.publish(twist) ``` Input parameters: **left_distance: Distance value on the left side;** **nter_distance: Distance value in center;** **right_distance: Distance value on the right side;** Decision criteria: If the absolute difference between any distance value and **`self.plane_high`** is greater than 0.02, proceed to the next part of the code. This means that if there is a significant difference between the detected distance and the expected distance (self.plane_high), the robot will take action. Action taken: Create a Twist object and set its angular velocity z value to 0.8. At the point, the robot will rotate quickly to avoid colliding with obstacles. Set **`self.turn`** to True, indicating that the robot is rotating. S et **`self.time_stamp`** to the current time plus 0.3 seconds, indicating the duration of the rotation action. Publish this twist to **`self.mecanum_pub`** to make the robot perform the rotation action. **Stop rotating:** If during rotation, the distance value matches the expected value, then stop rotating. First, get the current time and compare it with the previous timestamp. If the current time exceeds the previously set timestamp, it indicates that the rotation action has been completed. Set **`self.turn`** to **False** and publish a stop Twist. Also, update the timestamp to the current time plus 0.2 seconds, indicating the duration of the movement action. **Forward action:** If the robot is not rotating (i.e., self.turn is False), then execute the forward action. First, get the current time and compare it with the previous timestamp. If the current time exceeds the previously set timestamp, it indicates that the forward action has been completed. Publish a Twist to make the robot move forward. Also, update the timestamp to the current time plus 0.2 seconds, indicating the duration of the movement action. 10. Run the `run()` function: ```py def run(self): count = 0 while self.running: depth_image = self.image_queue.get(block=True) depth_color_map = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.45), cv2.COLORMAP_JET) cv2.circle(depth_color_map, (int((self.left_roi[2] + self.left_roi[3]) / 2), int((self.left_roi[0] + self.left_roi[1]) / 2)), 10, (0, 0, 0), -1) cv2.circle(depth_color_map, (int((self.center_roi[2] + self.center_roi[3]) / 2), int((self.center_roi[0] + self.center_roi[1]) / 2)), 10, (0, 0, 0), -1) cv2.circle(depth_color_map, (int((self.right_roi[2] + self.right_roi[3]) / 2), int((self.right_roi[0] + self.right_roi[1]) / 2)), 10, (0, 0, 0), -1) left_distance = self.get_roi_distance(depth_image, self.left_roi) center_distance = self.get_roi_distance(depth_image, self.center_roi) right_distance = self.get_roi_distance(depth_image, self.right_roi) ``` **Initialization:** Initialize the variable “ count ” to 0 for the subsequent debugging. **Main loop:** While self.running is an infinite loop. As long as the set.running is True, the loop continues. **Obtain depth image:** “**`self.image_queue.get(block=True)`**” is used to retrieve the dept images from the queue. This obtain the real-time image. **Color mapping:** Using the **`cv2.applyColorMap`** function from OpenCV to convert the depth image to a color image. **`cv2.convertScaleAbs`** is used to adjust the contrast of the image. The colormap used here is JET. **Drawing circle markers:** Using **`cv2.circle`** to draw three circles on the color depth image, corresponding to the left, center, and right regions of interest (ROI). These three regions represent different objects or parts of the scene. The coordinates of the circles' centers are obtained by calculating the center coordinates of each ROI. **Obtaining distance information:** Using the **`self.get_roi_distance`** method to obtain the average distance values within each ROI. It calculates the depth values of each pixel in the depth image, then computes the average distance from these values. Debug mode, debug = true: ```py if self.debug: count += 1 print(left_distance, center_distance, right_distance) if count > 50 and not math.isnan(left_distance) and not math.isnan(center_distance) and not math.isnan(right_distance): count = 0 self.plane_high = (left_distance + center_distance + right_distance)/3 common.save_yaml_data({'plane_high': self.plane_high}, os.path.join( os.path.abspath(os.path.join(os.path.split(os.path.realpath(__file__))[0], '../..')), 'config/plane_distance.yaml')) self.debug = False ``` If **`self.debug`** is **True**, then enter debug mode. After every 50 distance values are obtained, calculate the average distance of the three ROIs and store it in **`self.plane_high`**. Then, save this value to a configuration file. Once exiting debug mode (i.e., when count is reset to 0), no further debugging operations will be performed. Non-debug mode, debug = true: ```py else: if math.isnan(left_distance): left_distance = 0 if math.isnan(center_distance): center_distance = 0 if math.isnan(right_distance): right_distance = 0 self.move_policy(left_distance, center_distance, right_distance) ``` If not in debug mode, check each distance value for NaN (not a number). If it is NaN, set it to 0. Then, based on these distance values, call the **`self.move_policy`** method to determine how to move the object or robot. Displaying the resulting image and sending the robot's motion status: ```py cv2.imshow('depth_color_map', depth_color_map) k = cv2.waitKey(1) if k != -1: self.running = False self.mecanum_pub.publish(Twist()) rospy.signal_shutdown('shutdown') ``` This code “**`cv2.imshow('depth_color_map', depth_color_map)`**” uses the **imshow** function from OpenCV to display the processed depth image. The title of the image is **`depth_color_map`**', and the image itself is stored in the **`depth_color_map`** variable. This is typically used for real-time display or debugging purposes. This line of code “**`self.mecanum_pub.publish`**” publishes the target position for the movement of the robot's chassis to the ROS node. ### 11.3.2 Cross the Single-plank Bridge * **Overview** The crossing of the single-plank bridge by a robot involves various aspects of technology such as balance control and environmental perception. In order to maintain balance, the robot needs to process precise posture control capabilities, encompassing precise control of the center of gravity and limb joints angles. It is necessary to leverage robot’s dynamic model and control algorithms such as PID control and fuzzy logic. Additionally, environmental perception also plays an important role in this task because the robot needs to be able to perceive the real-time information about surrounding environment, including the width, height and slope of the single-plank bridge for posture adjustment. All in all, the realization of this functionality requires substantial experimentation and meticulous calibration. * **Operation Steps** > [!NOTE] > > **Note: the entered command should be case sensitive, and the “Tab” key can used to complemented the key words.** 1. Start JetRover and connect it to Nomachine remote control system. Regarding the Remote desktop tool installation and connection, please refer to “**[Appendix->Remote Desktop Software]()**.” 2. Double click on

This section will primarily focus on analyzing the program of crossing the single-plank bridge. The path to the source code of the program is as follow: **~/ros_ws/src/hiwonder_example/scripts/rgbd_function/cross_bridge_node.py** **1. Import Module** ```py #!/usr/bin/env python3 # encoding: utf-8 # @data:2023/12/18 # @author:aiden # 过独木桥 import os import cv2 import math import rospy import queue import signal import numpy as np from hiwonder_sdk import common from sensor_msgs.msg import Image from std_srvs.srv import SetBool from geometry_msgs.msg import Twist from hiwonder_servo_msgs.msg import MultiRawIdPosDur from hiwonder_servo_controllers import bus_servo_control ``` **`import cv2`**: Import the OpenCV library for image processing and computer vision tasks. **`import math`**: Import the math library for mathematical operations. **`import os`**: Import the os library for operating system interaction. **`import rospy`:** Import the ropy library for ROS function. **`import queue`**: Import the queue library for thread-safe queue operations. **`import signal`**: Import the signal library for handling signals, such as program termination. **`import numpy as np`**: Import the NumPy library for numerical computations. **`from hiwonder_sdk import common`**: Import the common module from the hiwonder_sdk module. **`from sensor_msgs.msg import Image`**: Import the Image message type from the sensor_msgs module for processing sensor image data. **`from std_srvs.srv import SetBool`**: Import the SetBool service type from the std_srvs module. **`from geometry_msgs.msg import Twist`**: Import the Twist message type from the geometry_msgs module for representing robot velocity. **`from hiwonder_servo_msgs.msg import MultiRawIdPosDur`:** Import the MultiRawIdPosDur message type from the hiwonder_servo_msgs module, related to servo control. **`from hiwonder_servo_controllers import bus_servo_control`**: Import bus_servo_control from the hiwonder_servo_controllers module for controlling the car chassis. **2. Class** ```py class CrossBridgeNode(): def __init__(self, name): rospy.init_node(name, anonymous=True) signal.signal(signal.SIGINT, self.shutdown) self.running = True self.turn = False self.plane_high = rospy.get_param('~bridge_plane_distance', 0.34) self.twist = Twist() self.time_stamp = rospy.get_time() self.image_queue = queue.Queue(maxsize=2) self.left_roi = [290, 300, 165, 175] self.center_roi = [290, 300, 315, 325] self.right_roi = [290, 300, 465, 475] servos_pub = rospy.Publisher('/servo_controllers/port_id_1/multi_id_pos_dur', MultiRawIdPosDur, queue_size=1) self.mecanum_pub = rospy.Publisher('/hiwonder_controller/cmd_vel', Twist, queue_size=1) rospy.sleep(0.2) bus_servo_control.set_servos(servos_pub, 1, ((1, 500), (2, 700), (3, 85), (4, 150), (5, 500), (10, 200))) rospy.sleep(1) rospy.wait_for_service('/depth_cam/set_ldp') rospy.ServiceProxy('/depth_cam/set_ldp', SetBool)(False) rospy.sleep(1) self.mecanum_pub.publish(Twist()) self.debug = rospy.get_param('~debug', False) rospy.Subscriber('/depth_cam/depth/image_raw', Image, self.depth_callback) self.run() ``` **Initialization：** Define a node instance and name it '**name**'. **Setting anonymous=True** indicates that this node is anonymous, it won't have an ROS name. The initialization function proceeds from top to bottom as follows: - Use the **`signal.signal()`** function to set up a handler for the SIGINT signal (typically generated by Ctrl+C), used for node shutdown. - Set **`self.running`** to True, indicating the node is running. - Set **`self.turn`** to **False**, but it is not further used. - Get the parameter named **~bridge_plane_distance** from the parameter server and set its default value to 0.34. - Initialize a **Twist** object, which is typically used to represent the velocity or direction of a robot. - Record the current timestamp. - Initialize a queue with a maximum capacity of 2 for storing images. Define three Region of Interest (ROI), namely left, middle, and right. These ROIs are used for extracting specific areas from the depth image. **Publisher:** 1. Create a publisher named **/servo_controllers/port_id_1/multi_id_pos_dur**, publishing messages of type **MultiRawIdPosDur**. 2. Create a publisher named **/hiwonder_controller/cmd_vel**, publishing **Twist** messages for controlling the car chassis. 3. Use the **`bus_servo_control.set_servos()`** function, also used for controlling the car chassis. **Waiting for Service:** \(1\) Use **`rospy.wait_for_service()`** to wait for the service named **/depth_cam/set_ldp** to become available. \(2\) Use **`rospy.ServiceProxy()`** to call the service and pass a **False** parameter. **Publishing Twist Message:** Publish an empty Twist message. **Setting Debug Mode**: Get the parameter named ~debug from the parameter server and set its default value to False. If this parameter is True, then set self.debug to True, indicating that the program is in debug mode. **Subscriber:** r**ospy.Subscriber('/depth_cam/depth/image_raw', Image, self.depth_callback)** subscribes to the topic named "**/depth_cam/depth/image_raw**" with message type Image. When an image is received, it calls the callback function **self.depth_callback**. **Execution:** Call the **`self.run()`** method, which is typically a method defined within a ROS node, used to start the node's loop. **Image Queue:** A queue is utilized to store the two most recent frames of depth images. ```py def depth_callback(self, ros_depth_image): depth_image = np.ndarray(shape=(ros_depth_image.height, ros_depth_image.width), dtype=np.uint16, buffer=ros_depth_image.data) if self.image_queue.full(): # 如果队列已满，丢弃最旧的图像 self.image_queue.get() # 将图像放入队列 self.image_queue.put(depth_image) ``` **`def depth_callback(self, ros_depth_image)`:** This line defines a method named **`depth_callback`**, which takes one parameter: **`ros_depth_image`**. This parameter represents a ROS image message containing depth image data. Function to Obtain Depth Distance Information within ROI Range ```py def get_roi_distance(self, depth_image, roi): roi_image = depth_image[roi[0]:roi[1], roi[2]:roi[3]] try: distance = round(float(np.mean(roi_image[np.logical_and(roi_image>0, roi_image<30000)])/1000), 3) except: distance = 0 return distance ``` **`def get_roi_distance(self, depth_image, roi)`:** The code “**`def get_roi_distance(self, depth_image, roi)`**” defines a method named `get_roi_distance`, which receives three parameters including self (representing the class instance itself), depth_image (representing the input depth image) and roi (representing the four-tuple of ROI, i.e, y1, y2, x1, x2\]) **roi_image = depth_image\[roi\[0\]:roi\[1\], roi\[2\]:roi\[3\]\]** The code “**roi_image = depth_image\[roi\[0\]:roi\[1\], roi\[2\]:roi\[3\]\]**” uses slicing to extract the region of interest from `depth_image` and assigns it to `roi_image`. **distance = round(float(np.mean(roi_image\[np.logical_and(roi_image\>0, roi_image\<30000)\])/1000), 3)** The code “**distance = round(float(np.mean(roi_image\[np.logical_and(roi_image\>0, roi_image\<30000)\])/1000), 3)**” first filters out the pixel values in roi_image that are greater than 0 and less than 30000. Then it calculates the average value of these pixel values and converts it to a floating-point number. Next, it divides the average value by 1000 to convert the units to meters. Finally, it uses the round function to round the result to three decimal places and assigns the result to distance. **distance = 0：** If the exception occurs, set distance as 0. **return distance：** Return the calculated distance value. **`Steering decision-making (move_policy)`** ```py def move_policy(self, left_distance, center_distance, right_distance): if abs(left_distance - self.plane_high) > 0.02: self.twist.angular.z = -0.1 elif abs(right_distance - self.plane_high) > 0.02: self.twist.angular.z = 0.1 else: self.twist.angular.z = 0 if abs(center_distance - self.plane_high) > 0.02: self.twist = Twist() self.runnning = False else: self.twist.linear.x = 0.2 self.mecanum_pub.publish(self.twist) ``` **Method Definition**: `def move_policy(self, left_distance, center_distance, right_distance)`: This defines a method named `move_policy`, which takes three parameters: `left_distance`, `center_distance`, and `right_distance`. **Checking the distance on the left:** Checking if the difference between `left_distance` and `self.plane_high` is greater than 0.02. If the above condition is true, then set **self.twist.angular.z = -0.1**, indicating that the robot should rotate at a speed of -0.1. **Checking the distance on the right:** elif abs(right_distance - self.plane_high) \> 0.02: If the above condition is true, then set **self.twist.angular.z = 0.1**, indicating that the robot should rotate at a speed of 0.1. **Checking the distance in the middle:** Checking if the difference between **`center_distance`** and **`self.plane_high`** is greater than 0.02. If the above condition is true, then reset `self.twist` to a new Twist object and set **`self.running`** to False. > [!NOTE] > > (Note: There is a spelling mistake here, it should be **`self.running`**). **Other Cases:** If none of the above conditions are met, then set **`self.twist.linear.x = 0.2`**, indicating that the robot should move forward at a speed of 0.2. **Other Cases:** If none of the above conditions are met, then set **`self.twist.linear.x = 0.2`**, indicating that the robot should move forward at a speed of 0.2. **Publish Twist Message:** Finally, use `self.mecanum_pub.publish(self.twist)` to publish the current Twist message in order to control the robot's movement. Run the **`self.run()`** function: ```py def run(self): count = 0 while self.running: depth_image = self.image_queue.get(block=True) depth_color_map = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.45), cv2.COLORMAP_JET) cv2.circle(depth_color_map, (int((self.left_roi[2] + self.left_roi[3]) / 2), int((self.left_roi[0] + self.left_roi[1]) / 2)), 10, (0, 0, 0), -1) cv2.circle(depth_color_map, (int((self.center_roi[2] + self.center_roi[3]) / 2), int((self.center_roi[0] + self.center_roi[1]) / 2)), 10, (0, 0, 0), -1) cv2.circle(depth_color_map, (int((self.right_roi[2] + self.right_roi[3]) / 2), int((self.right_roi[0] + self.right_roi[1]) / 2)), 10, (0, 0, 0), -1) left_distance = self.get_roi_distance(depth_image, self.left_roi) center_distance = self.get_roi_distance(depth_image, self.center_roi) right_distance = self.get_roi_distance(depth_image, self.right_roi) ``` **Initialization:** Initialize the variable “ **count** ” to 0 for the subsequent debugging. **Main loop:** While self.running is an infinite loop. As long as the `set.running` is True, the loop continues. **Obtain depth image:** “**`self.image_queue.get(block=True)`**” is used to retrieve the dept images from the queue. This obtain the real-time image. **Color mapping:** Using the **`cv2.applyColorMap`** function from OpenCV to convert the depth image to a color image. **`cv2.convertScaleAbs`** is used to adjust the contrast of the image. The colormap used here is JET. **Drawing circle markers:** Using **`cv2.circle`** to draw three circles on the color depth image, corresponding to the left, center, and right regions of interest (ROI). These three regions represent different objects or parts of the scene. The coordinates of the circles' centers are obtained by calculating the center coordinates of each ROI. **Obtaining distance information:** Using the **`self.get_roi_distance`** method to obtain the average distance values within each ROI. It calculates the depth values of each pixel in the depth image, then computes the average distance from these values. **Logic judgement:** ```py if self.debug: count += 1 print(left_distance, center_distance, right_distance) if count > 50 and not math.isnan(center_distance): count = 0 self.plane_high = center_distance common.save_yaml_data({'plane_high': self.plane_high}, os.path.join( os.path.abspath(os.path.join(os.path.split(os.path.realpath(__file__))[0], '../..')), 'config/bridge_plane_distance.yaml')) self.debug = False else: if math.isnan(left_distance): left_distance = 0 if math.isnan(center_distance): center_distance = 0 if math.isnan(right_distance): right_distance = 0 self.move_policy(left_distance, center_distance, right_distance) ``` **`if self.debug`**: This is a conditional statement. Only when self.debug is True. count += 1 \# Increment the value of count. **`print(left_distance, center_distance, right_distance)`:** Print the distances of the three ROIs. **`if count \> 50 and not math.isnan(center_distance)`:** This is another conditional statement; the following code block is executed when both of the following conditions are met: count is greater than 50 if count \> 50 and not math.isnan(center_distance): **`center_distance`,** not a number, typically indicating invalid data. `center_distance` is not NaN (i.e., not a number, typically indicating invalid data). **`count = 0` ：**Reset count to 0. **`self.plane_high = center_distance`**：Set self.plane_high to the value of center_distance. **common.save_yaml_data({'plane_high': self.plane_high}, os.path.join(...))：**Save a YAML file containing the key-value pair 'plane_high': self.plane_high. **`self.debug = False`：** Set self.debug to False, indicating that subsequent code will no longer enter this conditional block. **else:** If self.debug is not True, then execute the following code block Here, it checks if the three distance values (left, center, right) are NaN; if so, it sets them to 0. Then it calls the **`self.move_policy()`** method and passes the processed distance values. **`cv2.imshow('depth_color_map', depth_color_map)`:** Display the color depth map using OpenCV **`self.running = False`：**Set self.running to False to stop a loop or process. **`self.mecanum_pub.publish(Twist())`:** Publish a Twist message to the mecanum publisher for controlling the motion of the car chassis. ### 11.3.3 Object Tracking * **Overview** Object Tracking involves implementing vision-based object tracking and robot motion control. It combines functionalities such as image processing, point cloud processing, PID control, robot motion control, visualization, and interaction. It can be used to guide the robot to automatically track and approach specific objects. * **Operation Steps** > [!NOTE] > > **Note: the entered command should be case sensitive, and the “Tab” key can used to complemented the key words.** 1. Start JetRover and connect it to Nomachine remote control system. Regarding the Remote desktop tool installation and connection, please refer to “**[Appendix->Remote Desktop Software]()**.” 2. Double click on

Initialization function `def __init__(self, name)`: 1) ROS service and publisher initialization: Initialize ROS service clients, subscribers and publishers for communication with other ROS nodes. 2. Signal handing: use **signal.signal** to catch interrupt signals (such 5as, Ctrl+C) for safely shutting down the node. 3. PID controller initialization: `self.pid_x = pid.PID(1.5, 0, 0)`: create a PID controller for controlling in x-direction, with parameter settings P-1.5, I=0, D=0. `self.pid_y = pid.PID(1.5, 0, 0)`: create a PID controller for controlling in y-direction, with similar parameter settings. 4. Speed and distance parameters: `self.x_speed, self.y_speed = 0.007, 0.007`: set the base speeds in x and y directions. `self.stop_distance = 0.4`: set the stopping distance to 0.4 meters. When the distance between the object and the camera reaches this value, the robot will stop moving. `self.x_stop = -0.04`: set the stopping position in x direction. 5. Image processing parameters: `self.scale = 4`: Set the scale factor for image resizing to reduce computational load during image processing. `self.proc_size = [int(640/self.scale), int(480/self.scale)]`: Calculate the processed image size based on the scale factor. 6. Other settings: **self.haved_add = False**: initialize a flag to determine whether the point cloud has been added to the visualization tool. **self.get_point = False:** used to mark whether tracking points have been successfully obtained. **self.display = 1:** set whether to display point cloud visualization. **self.running = True: f**lag controlling the running loop. **self.pc_queue = queue.Queue(maxsize=1)**: create a queue to store processed point cloud data. 7) Point cloud clipping and visualization settings: Defined a Region of Interest (ROI) and used it to initialize “**self.vol**”, which is an Open3D SelectionPolygonVolume object used for clipping the point cloud. 8) Camera Subscription and Synchronization: Subscribed to RGB images, depth images, and camera information using `message_filters`, and synchronized these messages by time. ### 11.3.4 Callback Function: The **`multi_callback`** function handles synchronized images and depth data obtained from ROS subscriptions. It converts ROS image formats to NumPy arrays and performs point cloud processing using Open3D. 9. Point Cloud Processing: Utilize Open3D to convert RGB and depth images into a point cloud. Perform clipping and filtering on the point cloud to remove unnecessary parts (e.g., ground). 10. Image Format Conversion: Convert ROS `ros_rgb_image` and `ros_depth_image` to NumPy array format. This is because Open3D and OpenCV primarily operate using NumPy arrays. ```py def multi_callback(self, ros_rgb_image, ros_depth_image, depth_camera_info): ``` Using cv2.resize to adjust the image size to match the previously set `self.proc_size`. 11. Camera Intrinsic Settings: Utilize the intrinsic parameters (K matrix) from the depth camera information `depth_camera_info` to construct the Open3D PinholeCameraIntrinsic object. This is an important component of the camera model used in point cloud reconstruction. ```py intrinsic = o3d.camera.PinholeCameraIntrinsic(int(depth_camera_info.width / self.scale), int(depth_camera_info.height / self.scale), int(depth_camera_info.K[0] / self.scale), int(depth_camera_info.K[4] / self.scale), int(depth_camera_info.K[2] / self.scale), int(depth_camera_info.K[5] / self.scale)) ``` 12. Creating RGBD Images: Use Open3D to convert the resized RGB image and depth image into an RGBDImage object. This step is a prerequisite for generating point clouds. ```py o3d_image_rgb = o3d.geometry.Image(rgb_image) o3d_image_depth = o3d.geometry.Image(np.ascontiguousarray(depth_image)) ``` 13. Generating Point Clouds: Convert the RGBD image to a point cloud by calling `create_from_rgbd_image`. This step is resource-intensive and will consume considerable CPU resources. ```py # rgbd_function --> point_cloud rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(o3d_image_rgb, o3d_image_depth, convert_rgb_to_intensity=False) # cpu占用大 pc = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)#, extrinsic=extrinsic)ic) ``` 14. Point Cloud Clipping: Utilize the previously set self.vol (a SelectionPolygonVolume object) to clip the point cloud, removing unwanted regions (such as areas outside the ground). ```py # 裁剪 roi_pc = self.vol.crop_point_cloud(pc) ``` 15. Point Cloud Processing: If the clipped point cloud `roi_pc` contains points, further processing is performed. Utilize the `segment_plane` method to segment the largest plane (typically the ground) from the point cloud and retain the non-plane parts (inlier_cloud). ```py if len(roi_pc.points) > 0: # 去除最大平面，即地面, 距离阈4mm，邻点数，迭代次数 plane_model, inliers = roi_pc.segment_plane(distance_threshold=0.05, ransac_n=5, num_iterations=40) ``` 16. Updating Target Point Cloud: Set the processed point cloud (excluding the plane portion) as the class's self.target_cloud attribute for further processing. ```py if len(roi_pc.points) > 0: # 去除最大平面，即地面, 距离阈4mm，邻点数，迭代次数 plane_model, inliers = roi_pc.segment_plane(distance_threshold=0.05, ransac_n=5, num_iterations=40) # 保留内点 inlier_cloud = roi_pc.select_by_index(inliers, invert=True) self.target_cloud.points = inlier_cloud.points self.target_cloud.colors = inlier_cloud.colors else: self.target_cloud.points = roi_pc.points self.target_cloud.colors = roi_pc.colors # 转180度方便查看 self.target_cloud.transform(np.asarray([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])) try: self.pc_queue.put_nowait(self.target_cloud) ``` 17. Transforming and Storing Point Clouds: Transform the target point cloud (e.g., rotate by 180 degrees) for visualization purposes. Place the processed point cloud into the queue self.pc_queue for visualization or other processing purposes. ```py # 转180度方便查看 self.target_cloud.transform(np.asarray([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])) try: self.pc_queue.put_nowait(self.target_cloud) except queue.Full: pass ``` ### 11.3.5 Run Function 1) PID Control: Adjust the robot's movement using PID controllers to maintain the target object at a specific distance and position. The `pid_x` and `pid_y` controllers adjust the velocity in the x and y directions, respectively. 2) Visualization: If visualization is enabled (self.display == 1), display the processed point cloud using Open3D visualization tools. 3) Robot Control: Control the robot's movement by publishing Twist messages based on the processed point cloud and the output of the PID controllers. 4. PID Control: PID controllers adjust the robot's movement based on point cloud data to maintain the target object at a specific distance and position. The `self.pid_x` and `self.pid_y` controllers are used to adjust the velocity in the x and y directions. In the point cloud, the program finds the position of the farthest point (min_point) and updates the output of the PID controllers based on this position. ```py min_point = points[min_index] ``` ```py distance = min_point[-1] ``` The setpoint of the PID controllers is set to the predefined stop distance (`self.stop_distance`) and the stop position in the x-direction (`self.x_stop`). ```py self.pid_x.SetPoint = self.stop_distance ``` ```py self.pid_y.SetPoint = self.x_stop ``` By computing and adjusting `self.linear_x` and `self.linear_y`, the program dynamically adjusts the robot's movement speed in the x and y directions. ```py if abs(distance - self.stop_distance) < 0.1: distance = self.stop_distance self.pid_x.update(-distance) #更新pid tmp = self.x_speed - self.pid_x.output self.linear_x = tmp if tmp > 0.3: self.linear_x = 0.3 if tmp < -0.3: self.linear_x = -0.3 if abs(tmp) < 0.008: self.linear_x = 0 twist = Twist() twist.linear.x = self.linear_x ``` ```py if abs(y_distance - self.x_stop) < 0.03: y_distance = self.x_stop self.pid_y.update(y_distance) #更新pid tmp = self.y_speed + self.pid_y.output self.linear_y = tmp if tmp > 0.3: self.linear_y = 0.3 if tmp < -0.3: self.linear_y = -0.3 if abs(tmp) < 0.008: self.linear_y = 0 twist.linear.y = self.linear_y ``` 5. Visualization If self.display is set to 1, the program utilizes Open3D to create a visualization window and display the processed point cloud. The vis object represents Open3D's visualization tool, used for adding and updating point cloud geometries. In each iteration, the program retrieves new point cloud data from the queue `self.pc_queue` and updates the displayed point cloud using `vis.update_geometry`. ```py # print(min_point) if self.display: vis.update_geometry(point_cloud) vis.poll_events() vis.update_renderer() self.mecanum_pub.publish(twist) else: self.mecanum_pub.publish(Twist()) else: rospy.sleep(0.01) self.mecanum_pub.publish(Twist()) # 销毁所有显示的几何图形 vis.destroy_window() ``` 6. Robot Control Based on the output of the PID controllers, the program creates a Twist message and sets its linear velocity (twist.linear.x and twist.linear.y). This Twist message is then published to the ROS topic **/hiwonder_controller/cmd_vel**, which is used to control the robot's movement. If no valid point cloud data is detected, the program publishes an empty Twist message to stop the robot. ```py # print(min_point) if self.display: vis.update_geometry(point_cloud) vis.poll_events() vis.update_renderer() self.mecanum_pub.publish(twist) else: self.mecanum_pub.publish(Twist()) else: rospy.sleep(0.01) self.mecanum_pub.publish(Twist()) ``` 7. Others The while self.running loop ensures that the program continues running until self.running is set to False. At the end of the loop, the program cleans up resources, such as closing the visualization window, and safely shuts down the ROS node by calling rospy.signal_shutdown. ### 11.3.6 Tracking and Gripping * **Overview** Combining depth vision and robot control techniques, this system identifies and tracks objects of specific colors, completing precise grasping actions. It utilizes a depth camera to capture images and depth information, employs color tracking algorithms to locate the target object, and uses a robotic arm for object operations. Features include using the OpenCV library to identify objects of specific colors in images, computing the exact position of objects in three-dimensional space using depth information, adjusting the movement of the robotic arm using PID controllers to precisely track and approach the target object. This system can be applied in automated production lines to achieve precise object picking and placing tasks, such as in service robots and automation. * **Operation Steps** > [!NOTE] > > **Note: the entered command should be case sensitive, and the “Tab” key can used to complemented the key words.** 1. Start JetRover and connect it to Nomachine remote control system. Regarding the Remote desktop tool installation and connection, please refer to “**[Appendix->Remote Desktop Software]()**.” 2. Double click on

* Auxiliary function：**depth_pixel_to_camera** Translate the pixel coordinates from the depth camera to points in the camera coordinate system. ```py def depth_pixel_to_camera(pixel_coords, depth, intrinsics): fx, fy, cx, cy = intrinsics px, py = pixel_coords x = (px - cx) * depth / fx y = (py - cy) * depth / fy z = depth return np.array([x, y, z]) ``` - Class ColorTracker Class is used for tracking a specific color. Contains a PID controller used to adjust the position of the colored block in the image, aiming to keep it as close to the center of the image as possible. Proc method for image processing. It identities regions of specific color and adjust the central position of the colored block using a PID controller - **Initialization (\_\_init\_\_)** **target_color**: the target color to be tracked. pid_yaw and pid_pitch: these two PID controllers are respectively used for adjust the later (yaw) and vertical (pitch) positions of the object in the image, This allows the colored target object to remain centered in the image. yaw and pitch: these variables record the current yaw and pitch angles. - Image Processing Method (proc) **Image Preprocessing:** Firstly, the size of the source image is halved to reduce the computational load. Then, Gaussian blur (**cv2.GaussianBlur**) is applied to smooth the image, which helps reduce noise. Lastly, the image is converted from the RGB color space to the LAB color space (**cv2.cvtColor**). The LAB color space is more suitable for color segmentation. ```py def proc(self, source_image, result_image, color_ranges): h, w = source_image.shape[:2] color = color_ranges['lab']['Stereo'][self.target_color] img = cv2.resize(source_image, (int(w/2), int(h/2))) img_blur = cv2.GaussianBlur(img, (3, 3), 3) # 高斯模糊 img_lab = cv2.cvtColor(img_blur, cv2.COLOR_RGB2LAB) # 转换到 LAB 空间 ``` 1) Color Segmentation: Use **`cv2.inRange`** to perform binary thresholding on the LAB image, retaining only pixels within a specific color range. ```py mask = cv2.inRange(img_lab, tuple(color['min']), tuple(color['max'])) # 二值化 ``` 2) Morphological Processing: Perform erosion (**`cv2.erode`**) and dilation (**`cv2.dilate`**) operations on the binary image. This helps to remove small noise and highlight the main objects. ```py # 平滑边缘，去除小块，合并靠近的块 eroded = cv2.erode(mask, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))) dilated = cv2.dilate(eroded, cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))) ``` 3) Contour Detection: Utilize **`cv2.findContours`** to detect contours in the processed image. Iterate through the contours to find the most suitable one (typically the contour with the largest area). ```py contours = cv2.findContours(dilated, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[-2] ``` 4) Target Tracking and PID Control: If a suitable contour is found, compute its minimum enclosing circle (**`cv2.minEnclosingCircle`**) to determine the object's center position and size. ```py for c in contours: if math.fabs(cv2.contourArea(c)) < 50: continue (center_x, center_y), radius = cv2.minEnclosingCircle(c) # 最小外接圆 if min_c is None: min_c = (c, center_x) elif center_x < min_c[1]: if center_x < min_c[1]: min_c = (c, center_x) # 如果有符合要求的轮廓 if min_c is not None: (center_x, center_y), radius = cv2.minEnclosingCircle(min_c[0]) # 最小外接圆 ``` Utilize PID controllers to adjust the position of the object, aiming to keep it as close to the center of the image as possible. The **`pid_yaw`** controller adjusts the horizontal position (yaw), while the `pid_pitch` controller adjusts the vertical position (pitch). ```py # 圈出识别的的要追踪的色块 circle_color = common.range_rgb[self.target_color] if self.target_color in common.range_rgb else (0x55, 0x55, 0x55) cv2.circle(result_image, (int(center_x * 2), int(center_y * 2)), int(radius * 2), circle_color, 2) center_x = center_x * 2 center_x_1 = center_x / w if abs(center_x_1 - 0.5) > 0.02: # 相差范围小于一定值就不用再动了 self.pid_yaw.SetPoint = 0.5 # 我们的目标是要让色块在画面的中心, 就是整个画面的像素宽度的 1/2 位置 self.pid_yaw.update(center_x_1) self.yaw = min(max(self.yaw + self.pid_yaw.output, 0), 1000) else: self.pid_yaw.clear() # 如果已经到达中心了就复位一下 pid 控制器 ``` ```py center_y = center_y * 2 center_y_1 = center_y / h if abs(center_y_1 - 0.5) > 0.02: self.pid_pitch.SetPoint = 0.5 self.pid_pitch.update(center_y_1) self.pitch = min(max(self.pitch + self.pid_pitch.output, 100), 720) else: self.pid_pitch.clear() ``` 5) Return of Results: The method returns the processed image, current yaw and pitch angles, as well as the coordinates and radius of the object's center. ```py return (result_image, (self.pitch, self.yaw), (center_x, center_y), radius * 2) ``` - Class TrackAndGrapNode The main class of the program, responsible for initializing the ROS node, subscribing to image topics, processing images, controlling the robotic arm, etc. - Initialization (\_\_init\_\_) 1) Initialize the ROS node and set the logging level. Initialize some internal variables, such as **`self.moving`** (indicating whether the robotic arm is moving), **`self.start`** (indicating whether tracking has started), etc. ```py rospy.init_node(name, anonymous=True, log_level=rospy.INFO) self.fps = fps.FPS() self.moving = False self.count = 0 self.start = False self.running = True self.last_pitch_yaw = (0, 0) self.first_time = time.time() ``` 2. Set the initial position of the robotic arm. The function call **`bus_servo_control.set_servos`** is used to set the initial positions of each servo. The parameters ((1, 500), (2, 720), ...) represent the servo number and its initial position. ```py self.servos_pub = rospy.Publisher('/servo_controllers/port_id_1/multi_id_pos_dur', MultiRawIdPosDur, queue_size=1) rospy.sleep(0.2) bus_servo_control.set_servos(self.servos_pub, 1, ((1, 500), (2, 720), (3, 100), (4, 120), (5, 500), (10, 200))) rospy.sleep(1) ``` 3. Read and store the color configuration file. Utilize the **`common.get_yaml_data`** function to read the color configuration file, which contains LAB color space ranges for different colors used in color recognition. These configuration details are stored in the `self.lab_data` variable for later use in color tracking. ```py self.lab_data = common.get_yaml_data("/home/hiwonder/software/lab_tool/lab_config.yaml") ``` 4. Set up ROS services and subscribers, including services for starting and stopping tracking, as well as a service for color configuration. ```py rospy.Service('~start', Trigger, self.start_srv_callback) # 进入玩法 rospy.Service('~stop', Trigger, self.stop_srv_callback) # 退出玩法 rospy.Service('~set_color', SetString, self.set_color_srv_callback) ``` 5. Initialize an image queue for storing images received from the camera. ```py self.image_queue = queue.Queue(maxsize=2) ``` 6. Subscribe to ROS topics for RGB images, depth images, and camera information. ```py rgb_sub = message_filters.Subscriber('/depth_cam/rgb/image_raw', Image, queue_size=1) depth_sub = message_filters.Subscriber('/depth_cam/depth/image_raw', Image, queue_size=1) info_sub = message_filters.Subscriber('/depth_cam/depth/camera_info', CameraInfo, queue_size=1) ``` 7. Set up a time synchronizer (**ApproximateTimeSynchronizer**) to synchronize the received images and depth data. ```py # 同步时间戳, 时间允许有误差在0.03s sync = message_filters.ApproximateTimeSynchronizer([rgb_sub, depth_sub, info_sub], 3, 0.02) sync.registerCallback(self.multi_callback) #执行反馈函数 ``` 8. Method **shutdown**: When a termination signal is received, set self.running to False and safely shut down the program. 9. Methods `set_color_srv_callback`, `start_srv_callback`, `stop_srv_callback`: These methods serve as callback functions for ROS services, handling requests to start, stop, and set tracking color. 10. Method **`multi_callback`**: This is a ROS callback function used to handle synchronized RGB and depth images received from the camera. The received images are added to a queue for further processing. 11. Method **`get_endpoint`**: Invokes a ROS service to obtain the current position of the robotic arm's end effector. 12. Method **pick**: Controls the robotic arm to perform grasping actions based on the given position. Uses kinematics_control to calculate the grasping posture, then moves the robotic arm by controlling the servos. - Image Processing Flow 1. Method **`image_proc`**: This is the main loop of the program, responsible for processing images from the image queue. It uses the **ColorTracker** to process images, identifying and tracking objects of specific colors. It calculates the spatial position of objects and controls the robotic arm accordingly. The processed image and depth information are displayed. The image processing flow retrieves RGB and depth images from the queue. It converts the RGB image to BGR format for display in OpenCV. It applies color mapping to the depth image for visualizing depth information. If the color tracker is initialized and the robotic arm is not moving, it processes the image to track objects of specific colors. If an object is tracked and its position is stable, it calculates the object's world coordinates and initiates the grasping program. 2. Loop Processing **while self.running**: This loop ensures that the program continues running until a stop signal is received. 3. Get Image Data: Retrieve synchronized RGB images, depth images, and camera information from the image queue. ```py ros_rgb_image, ros_depth_image, depth_camera_info = self.image_queue.get(block=True) ``` Use np.ndarray to convert ROS image messages into NumPy array format for ease of further processing. ```py try: rgb_image = np.ndarray(shape=(ros_rgb_image.height, ros_rgb_image.width, 3), dtype=np.uint8, buffer=ros_rgb_image.data) depth_image = np.ndarray(shape=(ros_depth_image.height, ros_depth_image.width), dtype=np.uint16, buffer=ros_depth_image.data) ``` 4. Image Preprocessing: Convert RGB images to BGR format for proper display in OpenCV. Apply color mapping to depth images to generate depth color maps for visualizing depth information. ```py bgr_image = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR) ``` ```py sim_depth_image = np.clip(depth_image, 0, 2000).astype(np.float64) sim_depth_image = sim_depth_image / 2000.0 * 255.0 bgr_image = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR) depth_color_map = cv2.applyColorMap(sim_depth_image.astype(np.uint8), cv2.COLORMAP_JET) ``` 5. Color Tracking and Object Localization: If the color tracker (self.tracker) is initialized and the robotic arm is not in a moving state, start processing images to track objects of specific colors. Utilize the **`self.tracker.proc`** method to process RGB images, returning processed images, object positions, radii, and other information. ```py if self.tracker is not None and self.moving == False and time.time() > self.ttt and self.start: result_image, p_y, center, r = self.tracker.proc(rgb_image, result_image, self.lab_data) ``` 6. Object Position Stability Evaluation: If the object position remains stable for a certain period of time (for example, if there is minimal change in position between consecutive tracking results), then consider the object position stable enough for grasping. ```py if abs(self.last_pitch_yaw[0] - p_y[0]) < 3 and abs(self.last_pitch_yaw[1] - p_y[1]) < 3: if time.time() - self.stamp > 2: self.stamp = time.time() roi = [int(center_y) - 5, int(center_y) + 5, int(center_x) - 5, int(center_x) + 5] if roi[0] < 0: roi[0] = 0 if roi[1] > h: roi[1] = h if roi[2] < 0: roi[2] = 0 if roi[3] > w: roi[3] = w roi_distance = depth_image[roi[0]:roi[1], roi[2]:roi[3]] try: dist = round(float(np.mean(roi_distance[np.logical_and(roi_distance>0, roi_distance<10000)])/1000.0), 3) except BaseException as e: print(e) txt = "DISTANCE ERROR !!!" return if np.isnan(dist): txt = "DISTANCE ERROR !!!" return dist += 0.015 # 物体半径补偿 dist += 0.015 # 误差补偿 K = depth_camera_info.K self.get_endpoint() position = depth_pixel_to_camera((center_x, center_y), dist, (K[0], K[4], K[2], K[5])) position[0] -= 0.01 # rgb相机和深度相机tf有1cm偏移 pose_end = np.matmul(self.hand2cam_tf_matrix, common.xyz_euler_to_mat(position, (0, 0, 0))) # 转换的末端相对坐标 world_pose = np.matmul(self.endpoint, pose_end) # 转换到机械臂世界坐标 pose_t, pose_R = common.mat_to_xyz_euler(world_pose) self.stamp = time.time() self.moving = True threading.Thread(target=self.pick, args=(pose_t,)).start() else: self.stamp = time.time() ``` - Depth Data Processing: Calculate the actual position of objects in space based on their image coordinates and depth information. The calculation involves transforming from pixel coordinates to camera coordinates and accounting for the offset between the RGB camera and the depth camera. 1. Extracting Depth Information: Retrieve depth data from a specific region of interest (ROI) in the depth image. This is typically a small area around the object's position in the image. Calculate the average depth value within this region, representing the distance from the object to the camera. 2. Pixel Coordinates to Camera Coordinates Transformation: Use the depth_pixel_to_camera function to convert the object's image coordinates (pixel coordinates) to points in the camera coordinate system. This transformation requires considering the camera's intrinsic parameters (such as focal length and principal point), usually obtained from camera information. 3. Accounting for Offset Between RGB Camera and Depth Camera: If the RGB camera and depth camera are not the same device, there may be spatial offsets. The program compensates for this offset by adjusting the position coordinates and recalibrating the obtained coordinates. Finally, these coordinates are transformed into world coordinates for the robotic arm to execute subsequent grasping actions. ```py dist += 0.015 # 物体半径补偿 dist += 0.015 # 误差补偿 K = depth_camera_info.K self.get_endpoint() position = depth_pixel_to_camera((center_x, center_y), dist, (K[0], K[4], K[2], K[5])) position[0] -= 0.01 # rgb相机和深度相机tf有1cm偏移 pose_end = np.matmul(self.hand2cam_tf_matrix, common.xyz_euler_to_mat(position, (0, 0, 0))) # 转换的末端相对坐标 world_pose = np.matmul(self.endpoint, pose_end) # 转换到机械臂世界坐标 pose_t, pose_R = common.mat_to_xyz_euler(world_pose) ``` 4. Robotic Arm Grasping Action: Based on the calculated object position, control the robotic arm to perform grasping actions. This step is achieved by invoking the self.pick method in a new thread to avoid blocking the main loop. ```py threading.Thread(target=self.pick, args=(pose_t,)).start() ``` ```py def pick(self, position): if position[2] < 0.2: yaw = 80 else: yaw = 30 ret = kinematics_control.set_pose_target(position, yaw) if len(ret[1]) > 0: bus_servo_control.set_servos(self.servos_pub, 1, ((1, ret[1][0]), )) rospy.sleep(1) bus_servo_control.set_servos(self.servos_pub, 1.5, ((1, ret[1][0]),(2, ret[1][1]), (3, ret[1][2]),(4, ret[1][3]), (5, ret[1][4]))) rospy.sleep(1.5) bus_servo_control.set_servos(self.servos_pub, 0.5, ((10, 600),)) rospy.sleep(1) position[2] += 0.03 ret = kinematics_control.set_pose_target(position, yaw) if len(ret[1]) > 0: bus_servo_control.set_servos(self.servos_pub, 1, ((1, ret[1][0]),(2, ret[1][1]), (3, ret[1][2]),(4, ret[1][3]), (5, ret[1][4]))) rospy.sleep(1) bus_servo_control.set_servos(self.servos_pub, 1, ((1, 500), (2, 720), (3, 100), (4, 120), (5, 500), (10, 600))) rospy.sleep(1) bus_servo_control.set_servos(self.servos_pub, 1, ((1, 125), (2, 635), (3, 120), (4, 200), (5, 500))) rospy.sleep(1) bus_servo_control.set_servos(self.servos_pub, 1.5, ((1, 125), (2, 325), (3, 200), (4, 290), (5, 500))) rospy.sleep(1.5) bus_servo_control.set_servos(self.servos_pub, 1, ((1, 125), (2, 325), (3, 200), (4, 290), (5, 500), (10, 200))) rospy.sleep(1.5) bus_servo_control.set_servos(self.servos_pub, 1, ((1, 500), (2, 720), (3, 100), (4, 150), (5, 500), (10, 200))) rospy.sleep(2) self.tracker.yaw = 500 self.tracker.pitch = 150 self.tracker.pid_yaw.clear() self.tracker.pid_pitch.clear() self.stamp = time.time() self.first_time = time.time() + 2 self.moving = False ``` 5. Display Results: Concatenate the processed RGB image and depth color map together and display them in an OpenCV window. ```py if self.enable_disp: result_image = np.concatenate([cv2.cvtColor(result_image, cv2.COLOR_RGB2BGR), depth_color_map, ], axis=1) cv2.imshow("depth", result_image) key = cv2.waitKey(1) if key != -1: rospy.signal_shutdown('shutdown1') ``` 6. Exception Handling: Capture and log any exceptions to ensure the stability of the program. ```py while self.running: ros_rgb_image, ros_depth_image, depth_camera_info = self.image_queue.get(block=True) try: rgb_image = np.ndarray(shape=(ros_rgb_image.height, ros_rgb_image.width, 3), dtype=np.uint8, buffer=ros_rgb_image.data) depth_image = np.ndarray(shape=(ros_depth_image.height, ros_depth_image.width), dtype=np.uint16, buffer=ros_depth_image.data) result_image = np.copy(rgb_image) h, w = depth_image.shape[:2] depth = np.copy(depth_image).reshape((-1, )) depth[depth<=0] = 55555 sim_depth_image = np.clip(depth_image, 0, 2000).astype(np.float64) sim_depth_image = sim_depth_image / 2000.0 * 255.0 bgr_image = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR) depth_color_map = cv2.applyColorMap(sim_depth_image.astype(np.uint8), cv2.COLORMAP_JET) if self.tracker is not None and self.moving == False and time.time() > self.ttt and self.start: result_image, p_y, center, r = self.tracker.proc(rgb_image, result_image, self.lab_data) if p_y is not None: bus_servo_control.set_servos(self.servos_pub, 0.02, ((1, p_y[1]), (4, p_y[0]))) center_x, center_y = center if center_x > w: center_x = w if center_y > h: center_y = h if abs(self.last_pitch_yaw[0] - p_y[0]) < 3 and abs(self.last_pitch_yaw[1] - p_y[1]) < 3: if time.time() - self.stamp > 2: self.stamp = time.time() roi = [int(center_y) - 5, int(center_y) + 5, int(center_x) - 5, int(center_x) + 5] if roi[0] < 0: roi[0] = 0 if roi[1] > h: roi[1] = h if roi[2] < 0: roi[2] = 0 if roi[3] > w: roi[3] = w roi_distance = depth_image[roi[0]:roi[1], roi[2]:roi[3]] try: dist = round(float(np.mean(roi_distance[np.logical_and(roi_distance>0, roi_distance<10000)])/1000.0), 3) except BaseException as e: print(e) txt = "DISTANCE ERROR !!!" return if np.isnan(dist): txt = "DISTANCE ERROR !!!" return dist += 0.015 # 物体半径补偿 dist += 0.015 # 误差补偿 K = depth_camera_info.K self.get_endpoint() position = depth_pixel_to_camera((center_x, center_y), dist, (K[0], K[4], K[2], K[5])) position[0] -= 0.01 # rgb相机和深度相机tf有1cm偏移 pose_end = np.matmul(self.hand2cam_tf_matrix, common.xyz_euler_to_mat(position, (0, 0, 0))) # 转换的末端相对坐标 world_pose = np.matmul(self.endpoint, pose_end) # 转换到机械臂世界坐标 pose_t, pose_R = common.mat_to_xyz_euler(world_pose) self.stamp = time.time() self.moving = True threading.Thread(target=self.pick, args=(pose_t,)).start() else: self.stamp = time.time() dist = depth_image[int(center_y),int(center_x)] if dist < 100: txt = "TOO CLOSE !!!" else: txt = "Dist: {}mm".format(dist) cv2.circle(result_image, (int(center_x), int(center_y)), 5, (255, 255, 255), -1) cv2.circle(depth_color_map, (int(center_x), int(center_y)), 5, (255, 255, 255), -1) cv2.putText(depth_color_map, txt, (10, 400 - 20), cv2.FONT_HERSHEY_PLAIN, 2.0, (0, 0, 0), 10, cv2.LINE_AA) cv2.putText(depth_color_map, txt, (10, 400 - 20), cv2.FONT_HERSHEY_PLAIN, 2.0, (255, 255, 255), 2, cv2.LINE_AA) self.last_pitch_yaw = p_y else: self.stamp = time.time() if self.enable_disp: result_image = np.concatenate([cv2.cvtColor(result_image, cv2.COLOR_RGB2BGR), depth_color_map, ], axis=1) cv2.imshow("depth", result_image) key = cv2.waitKey(1) if key != -1: rospy.signal_shutdown('shutdown1') except Exception as e: rospy.logerr('callback error:', str(e)) ``` ### 11.3.7 Object Classification * **Overview** Object classification has widespread applications in industry, such as part sorting on production lines and goods classification in logistics warehouses. These applications require rapid and accurate identification and classification of objects to improve production efficiency and automation levels. In part sorting on production lines, robots can use machine vision and image processing techniques to identify different types of parts and sort them to different locations based on predefined classification criteria. This can enhance production efficiency and accuracy while reducing manual intervention and error rates. Furthermore, object classification has many other applications in industry, such as quality inspection, defect detection, and automated assembly. These applications also require rapid and accurate identification and classification of objects to ensure the stability and quality of the production process. This functionality simulates object classification in industrial applications, enabling robots to recognize objects of different shapes and colors in the current environment. * **Operation Steps** > [!NOTE] > > **Note: the entered command should be case sensitive, and the “Tab” key can used to complemented the key words.** 1. Start JetRover and connect it to Nomachine remote control system. Regarding the Remote desktop tool installation and connection, please refer to “**[Appendix->Remote Desktop Software]()**.” 2. Double click on

1. Import the relevant libraries ```py import os import sys import cv2 import math import rospy import queue import signal import threading import numpy as np import message_filters from std_srvs.srv import SetBool from hiwonder_sdk import pid, common, fps from xf_mic_asr_offline import voice_play from sensor_msgs.msg import Image, CameraInfo from position_change_detect import position_reorder from ros_robot_controller.msg import BuzzerState from std_srvs.srv import Trigger, TriggerResponse from hiwonder_interfaces.srv import SetStringList from hiwonder_kinematics import kinematics_control from hiwonder_servo_msgs.msg import MultiRawIdPosDur from hiwonder_servo_controllers import bus_servo_control sys.path.append('/home/hiwonder/software/arm_pc') from action_group_controller import ActionGroupController ``` **Import modules:** The code first imports some necessaries Python module. These module includes os, sys, cv2 (used for computer vision), math, rospy (ROS python client library), queue, signal, threading, numpy (for numerical computing), message_filters, and some custom modules. **ROS node:** Using rospy library, this code is part of a ROS node. ROS nodes are independent processes that perform specific tasks, and they interact with each other through ROS communication. **Threads and signal handling:** The code uses the threading and signal modules, indicating that it involves multi-threaded programming and signal handling. **Service calls:** The services imported from other modules, such as SetStringList and Trigger, indicate that this code involves interacting with ROS services. **Data Processing:** By importing various message types such as Image, CameraInfo, BuzzerState, MultiRawIdPosDur, etc., the code involves receiving and processing data from various sensors and devices. **Image Processing:** Since OpenCV is imported (represented by cv2), the code involves image processing, such as capturing, processing, or analyzing images from the camera. **Control Logic:** Importing modules related to robot control, such as pid, common, fps, kinematics_control, and bus_servo_control, indicates that the code involves robot motion control and logic. **Speech Broadcast:** By importing the voice_play module, the code includes functionality related to speech, such as playing pre-recorded audio messages. **Position Detection:** By importing the position_reorder module, the code involves detecting and reordering the positions of objects. 2. The mapping function of camera information in depth image (depth_pixel_to_camera). ```py def depth_pixel_to_camera(pixel_coords, intrinsic_matrix): fx, fy, cx, cy = intrinsic_matrix[0], intrinsic_matrix[4], intrinsic_matrix[2], intrinsic_matrix[5] px, py, pz = pixel_coords x = (px - cx) * pz / fx y = (py - cy) * pz / fy z = pz return np.array([x, y, z]) ``` **Function definition:** **`def depth_pixel_to_camera(pixel_coords, intrinsic_matrix)`:** This function receives two parameters: **`pixel_coords`:** an array including pixel coordinates, where px and py are the x and y coordinates of the pixel, and pz is the depth value corresponding to that pixel (typically obtained from a depth camera). **`intrinsic_matrix`：**the intrinsic matrix of the camera. This matrix usually includes four values: fx, fy, cx, and cy, where fx and fy are the focal lengths, and cx and cy are the image centers. **Extracting Values from the Intrinsic Matrix:** Here, four values are extracted from the intrinsic matrix, and they are assigned to fx, fy, cx, and cy respectively. **Calculating x and y Coordinates:** x = (px - cx) \* pz / fx y = (py - cy) \* pz / fy According to the pinhole camera model, the calculation of x and y coordinates involves transforming from pixel coordinates to points on the image plane. Here, the pixel coordinates are first centered (i.e., subtracting the image center), then multiplied by the depth value and divided by the corresponding focal length. **The z Coordinate Remains Unchanged:** z = pz Since the depth value is typically provided, the z coordinate remains unchanged. **Returning the Result:** return np.array(\[x, y, z\]) Finally, the calculated x, y, and z coordinates are combined into a numpy array and returned. - Class for Object Classification Execution (ObjectClassificationNode): ```py class ObjectClassificationNode: ``` Through this class, initialization of relevant parameters for object classification and execution of the object classification function can be achieved. ```py class ObjectClassificationNode: hand2cam_tf_matrix = [ [0.0, 0.0, 1.0, -0.101], [-1.0, 0.0, 0.0, 0.011], [0.0, -1.0, 0.0, 0.045], [0.0, 0.0, 0.0, 1.0] ] ``` This code defines a 4x4 transformation matrix: The first column represents scaling, rotation, and translation in the x-direction. Here, only scaling is performed (no rotation or translation). The second column represents scaling, rotation, and translation in the y-direction. Scaling (no rotation) and translation are performed here. The third column represents scaling, rotation, and translation in the z-direction. Similarly, only scaling is performed here (no rotation or translation). The fourth column represents the translation vector. Here, there is a translation of -0.101 in the x-direction, 0.011 in the y-direction, and 0.045 in the z-direction. - Picking Adjustment Parameters！！！ ```py pick_offset = [-0.01, -0.01, 0.0, -0.005, 0.0] # x1, x2, y1, y2, z ''' x1 y1 center y2 x2 arm car ''' ``` This parameter is an important configuration when using the object classification function. **`pick_offset`** is a 5-element list, consisting of: **-0.01, -0.01, 0.0, -0.005, 0.0.** Comments mention x1, x2, y1, y2, and z, indicating that these values are used for adjusting image coordinates or 3D coordinates. The diagram described in the comments illustrates a coordinate system, where: x1 and x2 are coordinates in the horizontal direction. y1 is the coordinate in the center vertical direction. y2 is the coordinate in the right vertical direction. z is the coordinate in the depth direction, representing the size of the grasping by the robotic arm. ```py def __init__(self, name): rospy.init_node(name, anonymous=True) self.fps = fps.FPS() self.moving = False self.count = 0 self.running = True self.start = False self.shapes = None self.colors = None self.target_shapes = '' self.roi = [30, 240, 170, 470] self.endpoint = None self.last_position = 0, 0 self.last_object_info_list = [] signal.signal(signal.SIGINT, self.shutdown) self.language = os.environ['ASR_LANGUAGE'] self.image_queue = queue.Queue(maxsize=2) self.lab_data = common.get_yaml_data("/home/hiwonder/software/lab_tool/lab_config.yaml") self.buzzer_pub = rospy.Publisher('/ros_robot_controller/set_buzzer', BuzzerState, queue_size=1) self.servos_pub = rospy.Publisher('/servo_controllers/port_id_1/multi_id_pos_dur', MultiRawIdPosDur, queue_size=1) self.controller = ActionGroupController(use_ros=True) rospy.wait_for_service('/depth_cam/set_ldp') rospy.wait_for_service('/kinematics/set_joint_value_target') rospy.sleep(0.2) self.goto_default() rospy.ServiceProxy('/depth_cam/set_ldp', SetBool)(False) rospy.Service('~start', Trigger, self.start_srv_callback) # 进入玩法 rospy.Service('~stop', Trigger, self.stop_srv_callback) # 退出玩法 rospy.Service('~set_shape', SetStringList, self.set_shape_srv_callback) rospy.Service('~set_color', SetStringList, self.set_color_srv_callback) rgb_sub = message_filters.Subscriber('/depth_cam/rgb/image_raw', Image, queue_size=1) depth_sub = message_filters.Subscriber('/depth_cam/depth/image_raw', Image, queue_size=1) info_sub = message_filters.Subscriber('/depth_cam/depth/camera_info', CameraInfo, queue_size=1) ``` The image above is a screenshot of the initialization function within the **ObjectClassificationNode** class. **rospy.init_node(name, anonymous=True)**: Initializes a ROS node where **name** is the name of the node, and **anonymous=True** indicates that the node is anonymous and will not be registered on the ROS master. **self.fps = fps.FPS()**: Creates an FPS object that can be used to calculate and display frame rates. **self.moving = False**: Initializes a boolean variable indicating whether the robot is currently moving. **self.count = 0**: Initializes a counter variable. **self.running = True**: Initializes a boolean variable indicating whether the program is running. **self.start = False:** Initialize a boolean variable indicating whether a certain operation or task has started. **self.shapes = None:** Initialize a variable for storing information about shapes. **self.colors = None:** Initialize a variable for storing information about colors. **self.target_shapes = '':** Initialize a string variable for storing information about the target shape. **self.roi = \[30, 240, 170, 470\]:** Initialize a list representing the coordinates of the Region of Interest (ROI). **self.endpoint = None:** Initialize a variable for storing the position or state of the end effector. **self.last_position = 0, 0:** Initialize a tuple representing the last recorded position. **self.last_object_info_list = \[\]:** Initialize a list for storing information about the last detected objects. **signal.signal(signal.SIGINT, self.shutdown):** Set up a signal handler to call self.shutdown method when receiving an interrupt signal (such as Ctrl+C). **self.language = os.environ\['ASR_LANGUAGE'\]:** Retrieve the language setting from the environment variable. **self.image_queue = queue.Queue(maxsize=2):** Create a queue for storing image messages with a maximum capacity of 2. **self.lab_data = common.get_yaml_data("/home/hiwonder/software/lab_tool/lab_config.yaml")**: Load a YAML configuration file from the specified path. **Create two publishers,** one for publishing BuzzerState messages and the other for MultiRawIdPosDur messages. **self.controller = ActionGroupController(use_ros=True):** Create an action group controller. **self.goto_default():** Invoke the `goto_default` method to move the robot to its default position or configuration. Invoke a service to set parameters for the depth camera. Define three services, each for starting, stopping, and setting callbacks for shape and color: **rospy.Service('~start', Trigger, self.start_srv_callback)** \# Enter game**rospy.Service('~stop', Trigger, self.stop_srv_callback)** \# Exit game **rospy.Service('~set_shape', SetStringList, self.set_shape_srv_callback)** **rospy.Service('~set_color', SetStringList, self.set_color_srv_callback)** Then create three subscribers for receiving RGB images, depth images, and camera information: rgb_sub = message_filters.Subscriber('/depth_cam/rgb/image_raw', Image, queue_size=1) depth_sub = message_filters.Subscriber('/depth_cam/depth/image_raw', Image, queue_size=1) info_sub = message_filters.Subscriber('/depth_cam/depth/camera_info', CameraInfo, queue_size=1) - Executing Parameter Retrieval ```py if rospy.get_param('~start', True): if rospy.get_param('~category', 'shape') == 'shape': msg = SetStringList() msg.data = ['sphere', 'cuboid', 'cylinder'] self.set_shape_srv_callback(msg) else: msg = SetStringList() msg.data = ['red', 'green', 'blue'] self.set_color_srv_callback(msg) ``` As shown in the screenshot above, after obtaining the 'start' parameter from the ROS environment, we need to call the corresponding function based on the incoming parameters. If category == shape, indicating the need to recognize shapes, the current service is set to request service messages. Otherwise, recognition is based on color. - Invocation of Image Processing Function (image_proc): This function involves a significant amount of code. Here, we'll outline the most important parts of the function, starting with the logic for object recognition and position determination, as illustrated in the diagram below: ```py if not self.moving: object_info_list = self.shape_recognition(rgb_image, depth_image, depth_color_map, depth_camera_info.K, min_dist) if self.start: reorder_object_info_list = object_info_list if object_info_list: if self.last_object_info_list: # 对比上一次的物体的位置来重新排序 reorder_object_info_list = position_reorder(object_info_list, self.last_object_info_list, 20) if reorder_object_info_list: if not self.target_shapes: if self.shapes is not None: indices = [i for i, info in enumerate(reorder_object_info_list) if info[0].split('_')[0] in self.shapes] else: indices = [i for i, info in enumerate(reorder_object_info_list) if self.color_comparison(info[-2]) in self.colors] if indices: min_depth_index = min(indices, key=lambda i: reorder_object_info_list[i][2]) self.target_shapes = reorder_object_info_list[min_depth_index][0].split('_')[0] ``` **if not self.moving:** This is a condition judgement. If self.moving is False, execute the following code block. **object_info_list = self.shape_recognition(rgb_image, depth_image, depth_color_map, depth_camera_info.K, min_dist):** Call the `self.shape_recognition` method and pass multiple parameters, including RGB image, depth image, depth color map. depth camera intrinsic parameters (camera matrix), and minimum distance. The purpose of this method is to recognize objects in the image and return a list containing object information. **if self.start:** If self.start is True, execute the following code block. **reorder_object_info_list = object_info_list** Assign the `object_info_list` obtained in the previous step to `reorder_object_info_list`, which copies the recognized object list to the final object information list. **if object_info_list:** If there are recognized objects, proceed with the following. **if self.last_object_info_list:** If `self.last_object_info_list` is not empty, execute the following code block. **reorder_object_info_list = position_reorder(object_info_list, self.last_object_info_list, 20)** Call the `position_reorder` function, passing the current object information list, the previous object information list, and 20 as parameters, and reassign the result to `reorder_object_info_list`. The purpose of this function is to reorder object information based on their positions. **if reorder_object_info_list:** If `reorder_object_info_list` is not empty, execute the following code block. **if not self.target_shapes:** If self.target_shapes is empty, execute the following code block. The following two conditional statements filter out certain object information: \- If self.shapes is not empty, it filters out object information matching the shapes in self.shapes. \- If self.colors is not empty, it filters out object information matching the colors in self.colors. **min_depth_index = min(indices, key=lambda i: reorder_object_info_list\[i\]\[2\])** Find the index of the object with the minimum depth from the filtered indices and assign it to `min_depth_index`. Here, the built-in function `min()` is used to find the minimum value. **self.target_shapes = reorder_object_info_list\[min_depth_index\]\[0\].split('\_')\[0\]** Assign the shape of the object with the minimum depth to `self.target_shapes`. Here, the split('\_') method is used to split the string, and the first part after splitting is retrieved. - Stability Judgment: ```py position = obejct_info[1] e_distance = round(math.sqrt(pow(self.last_position[0] - position[0], 2)) + math.sqrt( pow(self.last_position[1] - position[1], 2)), 5) if e_distance <= 0.005: self.count += 1 else: self.count = 0 if self.count > 5: self.count = 0 self.target_shapes = None self.moving = True # print( angle) if self.colors is not None: voice_play.play(self.color_comparison(obejct_info[-2]), language=self.language) else: voice_play.play(obejct_info[0].split('_')[0], language=self.language) threading.Thread(target=self.move, args=(obejct_info,)).start() self.last_position = position ``` After identifying an object, we need a short period of time to determine whether the position of the currently identified object is stable. The key factor in this determination is the depth error distance, denoted as e_distance, which represents the error distance. By using a formula, we can determine the distance between them as the sum of squares of the x and y-axis positions of the 3D depth information. Then, we check whether this distance is within a range of 0.005m. If it is, we pass its parameters to the thread function move, which controls the robotic arm to grip according to the position of the target block. ```py threading.Thread(target=self.move, args=(obejct_info,)).start() ``` - Drawing Result Images and Printing Messages: ```py cv2.rectangle(rgb_image, (self.roi[2], self.roi[0]), (self.roi[3], self.roi[1]), (255, 255, 0), 1) bgr_image = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR) self.fps.update() result_image = np.concatenate([depth_color_map, bgr_image], axis=1) cv2.imshow("depth", result_image) ``` Use the **`cv2.rectangle`** function to draw the minimum bounding rectangle of the currently recognized object. Perform spatial transformation on the BGR image by converting the RGB image to a BGR image. Finally, display the image using the **`cv2.imshow()`** function.