3
$\begingroup$

I'm using ROS2 foxy with no libraries installed. It runs on an Ubuntu 21.04 x64 machine with kernel 5.8.0-59-generic.

I created a project with the goal of controlling an Arduino using python serial in a workspace with the following structure:

- workspace
    - src
        - arduino_comm
             - serial_server.py
             - serial_client.py
        - motor_interface
        - other_interface

the motor interface provides the action that is used by the arduino_comm package to send serial data. I have an old Arduino Nano that I've had for like 7 years ago that uses the old legacy bootloader. Still operational but sadly seems incompatible with the rosserial and I've checked the Micro-Ros packages and they also seem incompatible with my old arduino. Also I wanted to understand the code I'm working with at its bare basics before adding more complexity with an external library.

So I created the following code.

Arduino code

//Variable to store the bot command.
String botCommand;

void setup() {
  //Initialize Serial port.
  Serial.begin(9600);
  //Initialize BUILTIN LED to light up for feedback.
  pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
  if(Serial.available()){
    botCommand = Serial.readString();
    if(botCommand == "forward"){
      //Light up the led.
      digitalWrite(LED_BUILTIN, HIGH);
      delay(10);
    }
    Serial.println(botCommand);
  }
  //Shut down the LED.
  digitalWrite(LED_BUILTIN, LOW);
  delay(10);
}

The Server and Client use the following action, which is based on the tutorial

##################################################
#Action that sends robot towards a specific spatial position.
##################################################
#Goal of achieving a specific position.
int32 botgoalposition
---
#Resulting position that we're aiming to attain.
int32[] botendposition
---
#Feedback message about current position.
int32[] botcurrposition

The arduino_comm serial_server.py dedicated to sending commands to the Arduino to follow.

import time
#Import the Serial library to send commands to Arduino.
import serial

#Elements required to generate an ActionServer.
import rclpy
from rclpy.action import ActionServer

#Library required to generate a ROS2 Node.
from rclpy.node import Node

##################################################
#                   ACTIONS
##################################################
#Action to calculate Fibonacci code. FOR DEBUGGING.
from code_interfaces.action import Fibonacci

#Action to send commands to the Arduino until it reaches the desired coordinates.
from bot_move.action import BotMove

#Class that encapsulates the Action Server.
class SerialCommServer(Node):
    def __init__(self):
        #Declare the Node with the name "serial server".
        super().__init__('serial_server')
        #Declare it'll be an ActionServer that publishes to the topic 'botmove'.
        self._action_server = ActionServer(
                self,
                BotMove,
                #Action name.
                'botmove',
                #Callback for executing accepted goals.
                self.executeCallback
                )

    #When it has been initialized, it'll start executing the following callback.
    def executeCallback(self, goalHandle):
        #Set the logger to indicate we're starting to execute our goal.
        self.get_logger().info('Executing goal...')

        #Get feedback to see if we're close to our goal.
        feedbackMsg = BotMove.Feedback()
        feedbackMsg.botcurrposition = [0,1]

        goalHandle.publish_feedback(feedbackMsg)

        #Send a small Hello to Arduino.
        arduino = serial.Serial(port="/dev/ttyUSB0")
        for i in range(1, 10):
            arduino.write(bytes(str("forward"), 'utf-8'))
            feedbackMsg.botcurrposition.append(i)
            self.get_logger().info('Feedback: {0}'.format(i))

            #Read back response from the Arduino.
            #TODO: THIS SECTION HANGS, NEED TO FIGURE HOW TO FIX IT.
#            response = arduino.readline()
#            print("Response: " + str(response))
            time.sleep(1)

#        for i in range(1, goalHandle.request.botgoalposition):
#            feedbackMsg.botcurrposition.append(
#                    feedbackMsg.botcurrposition[i] + feedbackMsg.botcurrposition[i-1])
#            #Show the position that has been achieved so far.
#            self.get_logger().info('Feedback: {0}'.format(feedbackMsg.botcurrposition))
#            goalHandle.publish_feedback(feedbackMsg)

        goalHandle.succeed()
        result = BotMove.Result()
        result.botendposition = feedbackMsg.botcurrposition

        #Returns result of the run. All Actions MUST return a result.
        return result

def main(args=None):
    rclpy.init(args=args)
    serialCommServer = SerialCommServer()

    try:
        print("Initializing Serial Communication Server.")
        rclpy.spin(serialCommServer)
    except KeyboardInterrupt:
        print("Keyboard interrupt command received")

if __name__ == '__main__':
    main()

And finally the client serial_client.py that doesn't do much of interest, later I want to send commands from another node but still need to fix this first.

import rclpy
from rclpy.action import ActionClient

#Library needed to generate a ROS2 Node.
from rclpy.node import Node

#Imports Action to solve a Fibonacci sequence. FOR DEBUGGING.
from code_interfaces.action import Fibonacci

#Action to manage position of the robot using Serial.
from bot_move.action import BotMove

#This is the Class that makes serial requests to the server.
class SerialCommClient(Node):
    def __init__(self):
        super().__init__('serial_client')

    #Declare a new Action Client.
    self._action_client = ActionClient(self, BotMove, 'botmove')

#This function waits for the action server to be available, then sends the new goal position the robot must occupy.
def sendGoal(self, botgoalposition):
    #Declare the BoMove action so we can start requesting a new robot position.
    goalMsg = BotMove.Goal()

    #Set a new goal for the action.
    goalMsg.botgoalposition = botgoalposition
    #Wait for the server to respond to the request.
    self._action_client.wait_for_server()
    self._send_goal_future = self._action_client.send_goal_async(
            goalMsg,
            feedback_callback=self.feedbackCallback
            )
    #When the goal has been set, execute this callback.
    self._send_goal_future.add_done_callback(self.goalResponseCallback)

#Goal Handle - Does different things depending on whether the server has accepted or rejected the goal.
def goalResponseCallback(self, future):
    goalHandle = future.result()

    if not goalHandle.accepted:
        #Log that the goal has been rejected.
        self.get_logger().info('Goal rejected :/')
        return
    #Otherwise, log that it has been accepted.
    self.get_logger().info('Goal accepted')

    self._get_result_future = goalHandle.get_result_async()
    #Callback to do something with the results we get after the server is done executing code.
    self._get_result_future.add_done_callback(self.getResultCallback)

#This callback function does things with the result.
def getResultCallback(self, future):
    result = future.result().result
    #Print the final position achieved.
    self.get_logger().info('Result: {0}'.format(result.botendposition))
    #Shutdown the Client.
    rclpy.shutdown()

#Gets the partial feedback portion of the message and prints it.
def feedbackCallback(self, feedbackMsg):
    feedback = feedbackMsg.feedback
    self.get_logger().info('Received feedback: {0}'.format(feedback.botcurrposition))

def main(args=None):
    rclpy.init(args=args)
    actionClient = SerialCommClient()
    #Establish the goal that will be reached by the server. In this case to reach some specific coordinates.
    actionClient.sendGoal(50)

    rclpy.spin(actionClient)

if __name__ == '__main__':
    main()

So far it works and properly sends the serial data to the Arduino which makes it blink. But when I try to read back the response it hangs. What could be a proper way to modify the code so the server can read serial data back? And is there a more proper way to build this? Thanks a lot for your help!

$\endgroup$
3
$\begingroup$

I found what I believe is a relevant answer here, and I think it's relevant because of the following lines in your serial_server.py file:

#When it has been initialized, it'll start executing the following callback.
def executeCallback(self, goalHandle):
    
    # <other code>
    
    #Send a small Hello to Arduino.
    arduino = serial.Serial(port="/dev/ttyUSB0")

The way I read this makes it look like you're starting a serial connection repeatedly. The post I linked states,

Establishing a serial connection to an arduino causes it to reset, so it was never online at the moment the pi was sending data, and so never replied.

There's a longer explanation about the behavior over at Arduino.SE that says, in part,

The Arduino uses the RTS (Request To Send) (and I think DTR (Data Terminal Ready)) signals to auto-reset.

The answer there also links here for instructions on disabling the auto-reset-on-serial-connection feature. Links can rot, so in the interest of future visitors I'll quote a subset of the page here, describing how to use a pull-up resistor on the reset pin to disable the auto-reset:

The simple way that doesn't require any permanent modifying of your hardware or software configuration changes:

Stick a 120 ohm resistor in the headers between 5v and reset (you can find these on the isp connector too). 120 is hard to find so just combine resistors. Don't go below 110 ohms or above 124 ohms, and don't do this with an isp programmer attached. You can just pull out the resistor when you want auto-reset back.

arduino pull-up resistor drawing

In summary, it looks like you're restarting your serial connection in every execute callback. Establishing the serial connection is resetting your arduino, and then it's resetting while you perform the serial write so it doesn't realize you're expecting a response when you get to the serial read.

You can avoid this by either starting the serial connection and storing the handle to the connection as a member variable or taking it as a callback parameter (preferred) or by disabling the auto-restart function.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.