Color interaction

1. Color interaction description

The snake-out-of-hole function uses the HSV color recognition function.

The path where the HSV color calibration file is saved is ~/jetcobot_ws/src/jetcobot_snake_follow/scripts/HSV_config.txt.

If the color recognition is not accurate enough, please open the ~/jetcobot_ws/src/jetcobot_snake_follow/scripts/HSV_calibration.ipynb file to calibrate the HSV value of the block color.

After the calibration operation is completed, it will be automatically saved to the HSV_config file. Rerun the program without additional code modification.

The main function of the snake-out-of-hole is to identify the selected color block and move forward and backward with the block. When it reaches the maximum distance range of the arm length, it will make the action of clamping the block.

2. About code

Code path：~/jetcobot_ws/src/jetcobot_snake_follow/scripts/snake_follow.ipynb

The function of controlling the forward and backward movement of the robotic arm.

    def snake_control(self, name, msg):
        for key, area in msg.items():
            if key == name:
                # x = round(self.pid.calculate(math.sqrt(area)), 2)
                x = round(self.pid.calculate(area), 2)
                coords = [230-x+80, -60, 300, -95, -44, -85]
                # coords = [230-x+80, -60, 300, -90, 50, -90]
                print("x=", 230-x+80)
                if not self.snake_clamp:
                    self.snake_check = self.snake_check + 1
                    if 230-x+80 >= 220:
                        if self.snake_check > 5:
                            self.snake_grip_block(name)
                            self.snake_check = 0
                    else:
                        self.mc.send_coords(coords, 100, 1)
                        self.snake_check = 0

The camera processes the image function, passes the image captured by the camera to the color interaction function for calculation, and drives the robot ic arm to move.

xxxxxxxxxx
def camera():
    # 打开摄像头 Open camera
    capture = cv.VideoCapture(0)
    capture.set(cv.CAP_PROP_FRAME_WIDTH, 640)
    capture.set(cv.CAP_PROP_FRAME_HEIGHT, 480)
    last_time = 0
    # Be executed in loop when the camera is opened normally 
    # 当摄像头正常打开的情况下循环执行
    while capture.isOpened():
        try:
            if model == 'Exit':
                break
            _, img = capture.read()
            # img = cv.resize(img, (640, 480))
            # # 获得运动信息 Get motion information
            img, snake_msg = snake_target.target_run(img, color_hsv, choose_color.value)
            if len(snake_msg) == 1 and time.time() - last_time > 0.7:
                snake_target.snake_control(choose_color.value, snake_msg)
                print("area:", snake_msg.get(choose_color.value))
                last_time = time.time()
            cv.putText(img, choose_color.value, (int(img.shape[0] / 2), 50), cv.FONT_HERSHEY_SIMPLEX, 2, color[random.randint(0, 254)], 2)
            imgbox.value = cv.imencode('.jpg', img)[1].tobytes()
            
        except KeyboardInterrupt:
            pass
        except Exception as e:
            print("except program:", e)
    capture.release()
    print("stop program")

3. Start program

Start roscore

• If you are using Jetson Orin NX/Jetson Orin Nano board. You need to enter the Docker environment using the following command.
• Then, run roscore

xxxxxxxxxx
sh ~/start_docker.sh
roscore

• If you are using Jetson Nano board. You need to enter the following command directly.

xxxxxxxxxx
roscore

Start the program

Open the jupyterlab webpage and find the corresponding .ipynb program file.

Click the Run button to run the entire code.

4. Experimental operation and results

After the program runs, the Jupyterlab webpage will display the control button.

Camera image on the left side, functions of the related buttons on the right side.

The robotic arm will control the gripper to loosen the clamp at regular intervals. At this time, put the corresponding color block into the camera screen, the robot arm will identify the corresponding color, and move the block back and forth, and the robot arm can be controlled to move forward and backward.

If the robotic arm is led to the farthest distance, the robot arm will first rotate the gripper to trigger the function of gripping the block, then open the gripper, put the block into the position where the gripper can grip, and the robot arm will grip the block to the selected color position.

If you need to exit the program, please click the 【Exit】 button.