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!