Introduction
The Rodney Robot project is a hobbyist robotic project to design and build an autonomous house-bot using ROS (Robot Operating System). This article is the fourth in the series describing the project.
Background
In part 1 to help define the requirements for our robot, we selected our first mission and split it down into a number of Design Goals to make it more manageable.
The mission was taken from the article Let's build a robot! and was:
Take a message to... - Since the robot will [have] the ability to recognize family members, how about the ability to make it the 'message taker and reminder'. I could say 'Robot, remind (PersonName) to pick me up from the station at 6pm'. Then, even if that household member had their phone turned on silent, or were listening to loud music or (insert reason to NOT pick me up at the station), the robot could wander through the house, find the person, and give them the message.
The design goals for this mission were:
- Design Goal 1: To be able to look around using the camera, search for faces, attempt to identify any people seen and display a message for any identified
- Design Goal 2: Facial expressions and speech synthesis. Rodney will need to be able to deliver the message
- Design Goal 3: Locomotion controlled by a remote keyboard and/or joystick
- Design Goal 4: Addition of a laser ranger finder or similar ranging sensor used to aid navigation
- Design Goal 5: Autonomous locomotion
- Design Goal 6: Task assignment and completion notification
In the previous parts of the article, we completed Design Goal 1 and 2. In this part, I'm going to introduce a state machine package and write two nodes which will be used to control the robot missions and jobs. To start bringing it all together, we will add a 2nd mission which makes use of Design Goal 1 and 2.
A Complex Plan
smach
When we are finally ready to bring all these Design Goals together, it's going to require a complex system to order and control all the various parts of the system. To do this, we are going to use a ROS Python library called smach. The package documentation is available on the ROS Wiki website smach.
With smach, we can develop a hierarchical state machine where we add a lower level state machine for each mission we add.
Gluing Design Goal 1 and 2 Together
Although our overall aim is what we have defined as Mission 1, it would be nice to start working on this control mechanism. What we can do is combine Design Goal 1 and 2 into another smaller mission (Mission 2) which is required to search for recognised faces within the head movement range and speak a greeting to anyone that the robot recognises. The processes used for Mission 2 will also form part of Mission 1 when it is complete.
To complete Mission 2, in this article, we are going to write two nodes. The first node, rodney_missions will contain the code for the state machine making up the missions and jobs. The second node rodney, will be used to control when various missions and jobs are started. We will also take the opportunity to add some functionality for reading the keyboard and a game controller which will be used in Design Goal 3.
Now I'm fully aware that I have introduced a new term there alongside "missions" and that is "jobs". A job will be a task that the robot needs to carry out, but is not as complex as a full mission. The node running the state machines is the best place for these "jobs" as they may require the same resources as the more complex missions. For example, the mission 1 state machine is required to request the movement of the head/camera but we may also want to be able to move the head/camera manually. Although it's fine to have two nodes subscribing to a topic, it's considered bad practice to have two nodes publishing the same topic. So we will avoid this by having one node to action the "missions" and "jobs".
Up to now, I have kept the node names generic and not named them after this particular robot. This was so that the nodes could be used in other projects, however these two nodes will be particular to this robot so they are named after it.
State Machine
We will start with the package and node which contains the state machine that controls the different missions and jobs that the robot is capable of. As stated above, smach is a Python library so our package will be written in Python.
Our ROS package for the node is called rodney_missions and is available in the rodney_missions folder. The src folder contains the rodney_missions_node.py file, which contains the main code for the node. The src folder also contains a sub folder called missions_lib, each of the robot missions we add to Rodney will result in a Python file which will be contained in this folder. Here, we are going to work on Mission 2 and the code for that is in the greet_all.py file.
The rodney_missions_node.py file will contain the code to register the node, some helper code and will also contain the high level state machine which accepts each mission and job. The greet_all.py file will contain the sub state machine for mission 2. Each time we add a new mission to the robot, we will add a sub state machine for that mission.
The diagram below shows our current state machine.
The WAITING state is a special type of state called a MonitorState and simply monitors the /missions/mission_request topic. When a message is received on this topic, it will extract the request and any parameters that go with the request and then transit to the PREPARE state passing on the request data.
The PREPARE state will carry out any 'Job' requests which may require to transit to the MOVE_HEAD, DEFAULT_HEAD_POSITION or back to the WAITING state. If the request was to carry out Mission 2 then it will transit to the sub state machine MISSION2.
The MOVE_HEAD state is a special type of state called a SimpleActionState. If this state is entered, then a request to move the head/camera to a given position will be sent. Once the move is complete, it will transit to the waiting state.
The DEFAULT_HEAD_POSITION state is also a SimpleActionState but this time, the request can only be to move to the default head/camera position. Once the move is complete, it will transit to the waiting state.
The REPORT state simply sends the mission complete message on the /missions/mission_complete topic and transits to the DEFAULT_HEAD_POSITION state.
The PREPARE_FOR_MOVEMENT_GRT state calculates what the next head position should be. If there are still scans to do, it will transit to the MOVE_HEAD_GRT state, otherwise if all head positions have been checked for known faces, then it will transit to the GREETING state.
The MOVE_HEAD_GRT is a SimpleActionState and will request the action to move the head to the position calculated by the previous state, it will then transit to the SCAN_FOR_FACES state.
The SCAN_FOR_FACES state is a SimpleActionState, but this one will request that a scan for known faces is conducted on the current image from the camera. If any faces are recognised, the names are returned in the result and are then stored for later use. The state then transits to the PREPARE_FOR_MOVEMENT_GRT.
The GREETING state will request a greeting is spoken for all the individuals recognised, it then transits to the report state.
If you look back to part two of these articles, we wrote two action clients to test the face_recognition and head_control nodes, in this code, the action clients are replaced by the Simple Action States.
Before I explain the code, it is worth stating what is contained in the /missions/mission_request topic. It is of type std_msgs/String and contains an ID for the Mission or the Job and depending on the ID, zero or more parameters separated by the '^' character.
Currently the IDs and parameters are as follows:
- "
M2" This ID is a request to conduct Mission 2 and there are no parameters associated with it. - "
J1" Is a request to conduct Job 1. This job is to playback the supplied wav file and to pass matching text to the robot face to animate the lips. The first parameter following the ID is the wav file name and the second parameter is the matching text. - "
J2" Is a request to conduct Job 2. This Job is to speak the supplied text and to pass the matching text to the robot face to animate the lips. The first parameter is the text to speak and the second parameter is the matching text. Remember these are separate as the text for the robot face may contain smileys for the robot face expression. - "
J3" Is a request to conduct Job 3. This Job is to move the position of the head/camera. The first parameter will contain the letter 'u' if the camera is to be moved up, 'd' if the camera is to be moved down, or '-' if the camera is not to be moved up or down. The second parameter contains 'l' if the camera is to be moved left, 'r' if the camera is to be moved right, or '-' if the camera is not to be moved left to right. The first parameter can also contain the letter 'c' if the head is to be moved to the default position.
I'll now briefly describe the code starting with the rodney_missions_node.py file. This file contains a main function, a class for the top level state machine, classes for the states that make up that state machine and a class that contains a number of helper functions that will be used by the various states.
The main function registers our node with ROS and creates an instance of the RodneyMissionsNode class.
def main(args):
rospy.init_node('rodney_missions_node', anonymous=False)
rospy.loginfo("Rodney missions node started")
rmn = RodneyMissionsNode()
if __name__ == '__main__':
main(sys.argv)
The class constructor for RodneyMissionsNode sets up to call ShutdownCallback if the node is shutdown. It then subscribes to the /missions/mission_cancel topic supplying the callback CnacelCallback. Next, it creates an instance of the helper class. It then creates the states of the top level state machine and adds then to state machine. At this level, the MISSION 2 sub state machine is a single state in our top level state machine.
We then create and start an introspective server. This is not required for the robot to operate but allows you to run a tool called smach_viewer. This tool can help to debug any problems with your state machine and was used to automatically produce the state machine diagram above.
The constructor then starts the execution of the state machine and hands control over to ROS.
There are three other functions in the RodneyMissionsNode class.
MissionsRequestCB is the function called by the MonitorState WAITING when a message is received on the /missions/mission_request topic. This extracts the data from the message and copies it to userdata which is a process for passing data between states. It then returns False so that the state machine will transit to the PREPARE state.
CancelCallback is the callback function called if a message is received on the /missions/mission_cancel topic. This will result in the pre-emption of our lower state machine if it is running.
ShutdownCallback is the callback function called if the node receives a command from ROS to shutdown.
class RodneyMissionsNode:
def __init__(self):
rospy.on_shutdown(self.ShutdownCallback)
self.__cancel_sub = rospy.Subscriber('/missions/mission_cancel', Empty,
self.CancelCallback)
self.__missions_helper = MissionsHelper()
self.__sm = StateMachine(outcomes=['preempted','aborted'],
output_keys=['mission_data'])
with self.__sm:
StateMachine.add('WAITING',
MonitorState('/missions/mission_request',
String,
self.MissionRequestCB,
output_keys = ['mission']),
transitions={'valid':'WAITING', 'invalid':'PREPARE',
'preempted':'preempted'})
StateMachine.add('PREPARE',
Prepare(self.__missions_helper),
transitions={'mission2':'MISSION2',
'done_task':'WAITING',
'head_default':'DEFAULT_HEAD_POSITION',
'move_head':'MOVE_HEAD'})
StateMachine.add('REPORT',
Report(),
transitions={'success':'DEFAULT_HEAD_POSITION'})
default_position_pan, default_position_tilt =
self.__missions_helper.CameraDefaultPos()
head_goal = point_headGoal()
head_goal.absolute = True
head_goal.pan = default_position_pan
head_goal.tilt = default_position_tilt
StateMachine.add('DEFAULT_HEAD_POSITION',
SimpleActionState('head_control_node',
point_headAction,
result_cb =
self.__missions_helper.CameraAtDefaultPos,
goal = head_goal),
transitions={'succeeded':'WAITING','preempted':'WAITING',
'aborted':'aborted'})
StateMachine.add('MOVE_HEAD',
SimpleActionState('head_control_node',
point_headAction,
goal_slots=['absolute', 'pan', 'tilt']),
transitions={'succeeded':'WAITING', 'preempted':'WAITING',
'aborted':'aborted'},
remapping={'absolute':'user_data_absolute', 'pan':'user_data_pan',
'tilt':'user_data_tilt'})
self.__sm_mission2 = missions_lib.Mission2StateMachine(self.__missions_helper)
StateMachine.add('MISSION2',
self.__sm_mission2,
transitions={'complete':'REPORT','preempted':'REPORT',
'aborted':'aborted'})
sis = IntrospectionServer('server_name', self.__sm, '/SM_ROOT')
sis.start()
self.__sm.execute()
rospy.spin()
sis.stop()
def MissionRequestCB(self, userdata, msg):
userdata.mission = msg.data
return False
def CancelCallback(self, data):
if self.__sm_mission2.is_running():
self.__sm_mission2.request_preempt()
def ShutdownCallback(self):
self.__sm.request_preempt()
The PREPARE and REPORT states are contained in separate classes within the rodney_missions_node.py file.
The class Prepare contains a constructor which declares which state follows PREPARE, what data is passed to it and also stores the instance of the helper class passed to it.
The class also contains an execute function which is run when the state is entered. This function examines the request message, carries out any Jobs it can and then transits to the WAITING state or will transit to another state to run the job or mission.
class Prepare(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['mission2','done_task','head_default','move_head'],
input_keys=['mission'],
output_keys=['mission_data','start',
'user_data_absolute','user_data_pan','user_data_tilt'])
self.__helper_obj = helper_obj
def execute(self, userdata):
retVal = 'done_task';
parameters = userdata.mission.split("^")
if parameters[0] == 'M2':
userdata.start = True
retVal = 'mission2'
elif parameters[0] == 'J1':
self.__helper_obj.Wav(parameters[1], parameters[2])
elif parameters[0] == 'J2':
self.__helper_obj.Speak(parameters[1], parameters[2])
elif parameters[0] == 'J3':
if 'c' in parameters[1]:
retVal = 'head_default'
else:
relative_request_pan, relative_request_tilt =
self.__helper_obj.CameraManualMove(parameters[1]+parameters[2])
userdata.user_data_absolute = False
userdata.user_data_pan = relative_request_pan
userdata.user_data_tilt = relative_request_tilt
retVal = 'move_head'
return retVal
The class Report contains a constructor which declares which state follows REPORT and advertises that it will publish a message on the /missions/mission_complete topic.
The class also contains an execute function which is run when the state is entered. This function simply publishes the message for the /missions/mission_complete topic.
class Report(State):
def __init__(self):
State.__init__(self, outcomes=['success'])
self.__pub = rospy.Publisher('/missions/mission_complete', String, queue_size=5)
def execute(self, userdata):
self.__pub.publish("Mission Complete")
return 'success'
The final class in the rodney_missions_node.py file is the MissionHelper class. This class contains code which the different missions will find useful and saves us duplicating the code in the various sub state machines we will eventually write. The functions will send messages to the voice and robot face nodes and calculate the next position of the head/camera.
class MissionsHelper():
def __init__(self):
self.__speech_pub_ = rospy.Publisher('/speech/to_speak', voice, queue_size=1)
self.__text_out_pub = rospy.Publisher('/robot_face/text_out', String, queue_size=1)
self.__pan_min = rospy.get_param("/servo/index0/pan/min", -(pi/2.0))
self.__pan_max = rospy.get_param("/servo/index0/pan/max", pi/2.0)
self.__tilt_min = rospy.get_param("/servo/index0/tilt/min", -(pi/2.0))
self.__tilt_max = rospy.get_param("/servo/index0/tilt/max", pi/2.0);
self.__camera_default_pan_position = rospy.get_param("/head/position/pan", 0.0)
self.__camera_default_tilt_position = rospy.get_param("/head/position/tilt", 0.0)
self.__pan_step_value = rospy.get_param("/head/view_step/pan", 0.436332)
self.__tilt_step_value = rospy.get_param("/head/view_step/tilt", 0.436332)
self.__manual_pan_step_value = rospy.get_param("/head/manual_view_step/pan", 0.174533)
self.__manual_tilt_step_value = rospy.get_param("/head/manual_view_step/tilt", 0.174533)
self.__increase_pan = True
self.__position_request_pan = self.__camera_default_pan_position
self.__position_request_tilt = self.__camera_default_tilt_position
def Speak(self, text_to_speak, text_to_display):
voice_msg = voice()
voice_msg.text = text_to_speak
voice_msg.wav = ""
self.__speech_pub_.publish(voice_msg)
self.__text_out_pub.publish(text_to_display)
def Wav(self, wav_file, text_to_display):
voice_msg = voice()
voice_msg.text = ""
voice_msg.wav = wav_file
self.__speech_pub_.publish(voice_msg)
self.__text_out_pub.publish(text_to_display)
def CameraToStartPos(self):
self.__position_request_pan = self.__pan_min
self.__position_request_tilt = self.__tilt_max
self.__increase_pan_ = True
return self.__position_request_pan, self.__position_request_tilt
def CameraAtDefaultPos(self, userdata, status, result):
if status == GoalStatus.SUCCEEDED:
self.__position_request_pan = self.__camera_default_pan_position
self.__position_request_tilt = self.__camera_default_tilt_position
def CameraDefaultPos(self):
return self.__camera_default_pan_position, self.__camera_default_tilt_position
def CameraToNextPos(self):
all_areas_scanned = False
if self.__increase_pan == True:
if self.__position_request_pan == self.__pan_max:
self.__increase_pan = False
self.__position_request_tilt -= self.__tilt_step_value
if self.__position_request_tilt < self.__tilt_min:
all_areas_scanned = True
else:
self.__position_request_pan += self.__pan_step_value
if self.__position_request_pan > self.__pan_max:
self.__position_request_pan = self.__pan_max
else:
if self.__position_request_pan == self.__pan_min:
self.__increase_pan = True
self.__position_request_tilt -= self.__tilt_step_value
if self.__position_request_tilt < self.__tilt_min:
all_areas_scanned = True
else:
self.__position_request_pan -= self.__pan_step_value
if self.__position_request_pan < self.__pan_min:
self.__position_request_pan = self.__pan_min
if all_areas_scanned == True:
self.__position_request_pan = self.__camera_default_pan_position
self.__position_request_tilt = self.__camera_default_tilt_position
return all_areas_scanned, self.__position_request_pan, self.__position_request_tilt
def CameraManualMove(self, direction):
relative_request_pan = 0.0
relative_request_tilt = 0.0
if 'd' in direction:
relative_request_tilt = self.__manual_tilt_step_value
if (self.__position_request_tilt + relative_request_tilt) > self.__tilt_max:
relative_request_tilt = self.__tilt_max - self.__position_request_tilt
self.__position_request_tilt = self.__tilt_max
else:
self.__position_request_tilt += relative_request_tilt
if 'u' in direction:
relative_request_tilt = -(self.__manual_tilt_step_value)
if (self.__position_request_tilt + relative_request_tilt) < self.__tilt_min:
relative_request_tilt = self.__tilt_min - self.__position_request_tilt
self.__position_request_tilt = self.__tilt_min
else:
self.__position_request_tilt += relative_request_tilt
if 'l' in direction:
relative_request_pan = self.__manual_pan_step_value
if (self.__position_request_pan + relative_request_pan) > self.__pan_max:
relative_request_pan = self.__pan_max - self.__position_request_pan
self.__position_request_pan = self.__pan_max
else:
self.__position_request_pan += relative_request_pan
if 'r' in direction:
relative_request_pan = -(self.__manual_pan_step_value)
if (self.__position_request_pan + relative_request_pan) < self.__pan_min:
relative_request_pan = self.__pan_min - self.__position_request_pan
self.__position_request_pan = self.__pan_min
else:
self.__position_request_pan += relative_request_pan
return relative_request_pan, relative_request_tilt
Each sub state machine we write for the various missions will be contained in the sub folder missions_lib. For now, we only have mission 2 and the code for this sub state machine is held in greet_all.py.
The main class in this file is Mission2StateMachine, this is a derived class from the StateMachine parent class. The constructor initialises the sub state machine, stores an instance of the helper class and creates each state that make up this sub state machine. The class also contains a callback function that is called when the face recognition action returns its result.
class Mission2StateMachine(StateMachine):
def __init__(self, helper_obj):
StateMachine.__init__(self, outcomes=['complete','preempted','aborted'],
input_keys=['start'])
self.__helper_obj = helper_obj
with self:
StateMachine.add('PREPARE_FOR_MOVEMENT_GRT',
PrepareMovementGeeting(self.__helper_obj),
transitions={'complete':'GREETING','move':'MOVE_HEAD_GRT'},
remapping={'start_in':'start','start_out':'start'})
StateMachine.add('MOVE_HEAD_GRT',
SimpleActionState('head_control_node',
point_headAction,
goal_slots=['absolute','pan','tilt']),
transitions={'succeeded':'SCAN_FOR_FACES',
'preempted':'preempted','aborted':'aborted'},
remapping={'absolute':'user_data_absolute',
'pan':'user_data_pan','tilt':'user_data_tilt'})
StateMachine.add('SCAN_FOR_FACES',
SimpleActionState('face_recognition',
scan_for_facesAction,
result_cb=self.face_recognition_result_cb,
input_keys=['seen_dict_in'],
output_keys=['seen_dict_out']),
remapping={'seen_dict_in':'seen_dict',
'seen_dict_out':'seen_dict'},
transitions={'succeeded':'PREPARE_FOR_MOVEMENT_GRT',
'preempted':'preempted','aborted':'aborted'})
StateMachine.add('GREETING',
Greeting(self.__helper_obj),
transitions={'complete':'complete'})
def face_recognition_result_cb(self, userdata, status, result):
if status == GoalStatus.SUCCEEDED:
local_dict = userdata.seen_dict_in
if len(result.ids_detected) > 0:
for idx, val in enumerate(result.ids_detected):
if val not in local_dict:
local_dict[val] = result.names_detected[idx]
rospy.loginfo("Greeting: I have seen %s", result.names_detected[idx])
userdata.seen_dict_out = local_dict
Also within the greet_all.py file are the two classes that make up the PREPARE_FOR_MOVEMENT_GRT and GREETING states.
class PrepareMovementGeeting(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['complete','move'],
input_keys=['start_in'],
output_keys=['start_out','seen_dict',
'user_data_absolute','user_data_pan','user_data_tilt'])
self.__helper_obj = helper_obj
def execute(self, userdata):
if userdata.start_in == True:
userdata.start_out = False
userdata.seen_dict = {}
scan_complete = False
position_request_pan, position_request_tilt =
self.__helper_obj.CameraToStartPos()
else:
scan_complete, position_request_pan, position_request_tilt =
self.__helper_obj.CameraToNextPos()
userdata.user_data_absolute = True
userdata.user_data_pan = position_request_pan
userdata.user_data_tilt = position_request_tilt
if scan_complete == True:
next_outcome = 'complete'
else:
next_outcome = 'move'
return next_outcome
class Greeting(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['complete'],
input_keys=['seen_dict'])
self.__helper_obj = helper_obj
def execute(self, userdata):
greeting = ''
if len(userdata.seen_dict) == 0:
greeting = 'No one recognised'
else:
greeting = 'Hello '
for id, name in userdata.seen_dict.items():
greeting += name + ' '
greeting += 'how are you '
if len(userdata.seen_dict) == 1:
greeting += 'today'
elif len(userdata.seen_dict) == 2:
greeting += 'both'
else:
greeting += 'all'
rospy.loginfo(greeting)
self.__helper_obj.Speak(greeting, greeting + ':)')
return 'complete'
As we add new missions to the project, we will follow this template of adding sub state machines to the library.
Top Level Control
The rodney node will be responsible for the top level control of the robot.
Our ROS package for the node is called rodney and is available in the rodney folder. The package contains all the usual ROS files and folders plus a few extra.
The config folder contains a config.yaml file which can be used to override some of the default configuration values. You can configure:
- the game controller axis which is used for moving the robot forward and backward in manual locomotion mode
- the game controller axis which is used for moving the robot clockwise and anti-clockwise in manual locomotion mode
- the game controller axis which will be used for moving the head/camera up and down in manual locomotion mode
- the game controller axis which will be used for moving the head/camera left and right in manual locomotion mode
- the game controller button which will be used for selecting manual locomotion mode
- the game controller button which will be used for moving the head/camera back to the default position
- the game controller axes dead zone value
- the linear velocity which is requested when the controller axis is at its maximum range
- the angular velocity which is requested when the controller axis is at its maximum range
- the ramp rate used to increase or decrease the linear velocity
- the ramp rate used to increase or decrease the angular velocity
- the battery voltage level that a low battery warning will be issued at
- enable/disable the wav file playback functionality when the robot is inactive
- a list of wav filenames to play from when the robot is inactive
- A list of speeches to use when playing the wav filesnames
The launch folder contains two launch files, rodney.launch and rviz.launch. The rodney.launch file is used to load all the configuration files, covered in the first four articles, into the parameter server and to start all the nodes that make up the robot project. It is similar to the launch files used so far in the project except it now includes the rodney_node and the rodney_missions_node. rviz is a 3D visualization tool for ROS which can be used to visualise data including the robot position and pose. Documentation for rviz is available on the ROS Wiki website. The rviz.launch file along with the meshes, rviz and urdf folders can be used for visualising Rodney. We will use the urdf model of Rodney to do some testing on a simulated Rodney robot.
The image below shows a visualisation of Rodney in rviz.
The rodney_control folder is just a convenient place to store the Arduino file that was discussed in Part 1.
The sounds folder is used to hold any wav files that the system is required to play. How to play these files and at the same time animate the robot face was covered in Part 3.
The include/rodney and src folders contain the C++ code for the package. For this package, we have one C++ class, RodneyNode, and a main routine contained within the rodney_node.cpp file.
The main routine informs ROS of our node, creates an instance of the node class and passes it the node handle.
Again, we are going to do some processing of our own in a loop so instead of passing control to ROS with a call to ros::spin, we are going to call ros::spinOnce to handle the transmitting and receiving of the topics. The loop will be maintained at a rate of 20Hz, this is setup by the call to ros::rate and the timing is maintained by the call to r.sleep within the loop.
Our loop will continue while the call to ros::ok returns true, it will return false when the node has finished shutting down, e.g., when you press Ctrl-c on the keyboard.
In the loop, we will call checkTimers and sendTwist which are described later in the article:
int main(int argc, char **argv)
{
ros::init(argc, argv, "rodney");
ros::NodeHandle n;
RodneyNode rodney_node(n);
std::string node_name = ros::this_node::getName();
ROS_INFO("%s started", node_name.c_str());
ros::Rate r(20);
while(ros::ok())
{
rodney_node.sendTwist();
rodney_node.checkTimers();
ros::spinOnce();
r.sleep();
}
return 0;
}
The constructor for our class starts by setting default values for the class parameters. For each of the parameters which are configurable using the ROS parameter server, a call is made to either param or getParam. The difference between these two calls is that with param, the default value passed to the call is used if the parameter is not available in the parameter server.
We next subscribe to the topics that the node is interested in.
- keyboard/keydown - to obtain key presses from a keyboard. These key presses are generated from a remote PC to control the robot in manual mode
- joy - to obtain joystick/game pad controller input, again to control the robot from a remote PC
- missions/mission_complete - so that the node is informed when the current robot mission is completed
- main_battery_status - this will be used later in the project to receive the state of the robots main battery
- demand_vel - this will be used later in the project to receive autonomous velocity demands
Next in the constructor is the advertisement of the topics which this node will publish.
- /robot_face/expected_input - This topic was discussed in part 3 of these articles and is used to display a status below the robot face. We will use it to show the status of the main battery
- /missions/mission_request - This will be used to pass requested missions and jobs on to the state machine node.
- /missions/mission_cancel - This can be used to cancel the current ongoing mission.
- /missions/mission_acknowledge - This will be used later in the project to acknowledge that part of a mission is complete and to continue with the rest of the mission.
- /cmd_vel - This will be used later in the project to send velocity commands to the node responsible for driving the electric motors. The requested velocities will either be from the autonomous subsystem or as a result of keyboard/joystick requests when in manual mode.
- /commands/reset_odometry - This will be used later in the project to reset the robot odometry values.
Finally, the constructor sets a random generator seed and obtains the current time. The use of the random number generator and the time is discussed in the section on the checkTimers method.
RodneyNode::RodneyNode(ros::NodeHandle n)
{
nh_ = n;
joystick_linear_speed_ = 0.0f;
joystick_angular_speed_ = 0.0f;
linear_mission_demand_ = 0.0f;
angular_mission_demand_ = 0.0f;
manual_locomotion_mode_ = false;
linear_set_speed_ = 0.5f;
angular_set_speed_ = 2.5f;
nh_.param("/controller/axes/linear_speed_index", linear_speed_index_, 0);
nh_.param("/controller/axes/angular_speed_index", angular_speed_index_, 1);
nh_.param("/controller/axes/camera_x_index", camera_x_index_, 2);
nh_.param("/controller/axes/camera_y_index", camera_y_index_, 3);
nh_.param("/controller/buttons/manual_mode_select", manual_mode_select_, 0);
nh_.param("/controller/buttons/default_camera_pos_select", default_camera_pos_select_, 1);
nh_.param("/controller/dead_zone", dead_zone_, 2000);
nh_.param("/teleop/max_linear_speed", max_linear_speed_, 3.0f);
nh_.param("/teleop/max_angular_speed", max_angular_speed_, 3.0f);
nh_.param("/motor/ramp/linear", ramp_for_linear_, 5.0f);
nh_.param("/motor/ramp/angular", ramp_for_angular_, 5.0f);
nh_.param("/battery/warning_level", voltage_level_warning_, 9.5f);
nh_.param("/sounds/enabled", wav_play_enabled_, false);
nh_.getParam("/sounds/filenames", wav_file_names_);
nh_.getParam("/sounds/text", wav_file_texts_);
key_sub_ = nh_.subscribe("keyboard/keydown", 5, &RodneyNode::keyboardCallBack, this);
joy_sub_ = nh_.subscribe("joy", 1, &RodneyNode::joystickCallback, this);
mission_sub_ = nh_.subscribe("/missions/mission_complete", 5,
&RodneyNode::completeCallBack, this);
battery_status_sub_ = nh_.subscribe("main_battery_status", 1,
&RodneyNode::batteryCallback, this);
demand_sub_ = nh_.subscribe("demand_vel", 5, &RodneyNode::motorDemandCallBack, this);
face_status_pub_ = nh_.advertise<std_msgs::String>("/robot_face/expected_input", 5);
mission_pub_ = nh_.advertise<std_msgs::String>("/missions/mission_request", 10);
cancel_pub_ = nh_.advertise<std_msgs::Empty>("/missions/mission_cancel", 5);
ack_pub_ = nh_.advertise<std_msgs::Empty>("/missions/acknowledge", 5);
twist_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
reset_odom_ = nh_.advertise<std_msgs::Empty>("/commands/reset_odometry", 1);
battery_low_count_ = 0;
mission_running_ = false;
srand((unsigned)time(0));
last_interaction_time_ = ros::Time::now();
}
I'll now briefly describe the functions that make up the class.
The joystickCallback is called when a message is received on the Joy topic. The data from the joystick/game pad controller can be used to move the robot around and to move the head/camera when in manual mode.
Data from the joystick is in two arrays, one contains the current position of each axes and the other the current state of the buttons. Which axis and which button are used is configurable by setting the index value in the parameter server.
The function first reads the axes that control the angular and linear speed of the robot. These values are compared to a dead zone value which dictates how much the axes must be moved before the value is used to control the robot. The values from the controller are then converted to values that can be used for linear and velocity demands. This will mean that the maximum possible value received from the controller will result in a demand of the robots top speed. These values are stored and will be used in the sendTwist method.
Next the axes used for controlling the movement of the head/camera in manual mode are read, again a dead zone is applied to the value. If the robot is in manual locomotion mode the values are sent as a "J3" job to the rondey_mission_node.
Next the button values are checked. Again the index of the button used for each function can be configured. One button is used to put the robot in manual locomotion mode, which if a robot mission is currently running results in a request to cancel the mission. The second button is used as a quick way of returning the head/camera to the default position.
void RodneyNode::joystickCallback(const sensor_msgs::Joy::ConstPtr& msg)
{
float joystick_x_axes;
float joystick_y_axes;
joystick_x_axes = msg->axes[angular_speed_index_];
joystick_y_axes = msg->axes[linear_speed_index_];
if(abs(joystick_x_axes) < dead_zone_)
{
joystick_x_axes = 0;
}
if(abs(joystick_y_axes) < dead_zone_)
{
joystick_y_axes = 0;
}
if(joystick_y_axes != 0)
{
joystick_linear_speed_ = -(joystick_y_axes*(max_linear_speed_/(float)MAX_AXES_VALUE_));
last_interaction_time_ = ros::Time::now();
}
else
{
joystick_linear_speed_ = 0;
}
if(joystick_x_axes != 0)
{
joystick_angular_speed_ =
-(joystick_x_axes*(max_angular_speed_/(float)MAX_AXES_VALUE_));
last_interaction_time_ = ros::Time::now();
}
else
{
joystick_angular_speed_ = 0;
}
joystick_x_axes = msg->axes[camera_x_index_];
joystick_y_axes = msg->axes[camera_y_index_];
if(abs(joystick_x_axes) < dead_zone_)
{
joystick_x_axes = 0;
}
if(abs(joystick_y_axes) < dead_zone_)
{
joystick_y_axes = 0;
}
if(manual_locomotion_mode_ == true)
{
if((joystick_x_axes != 0) || (joystick_y_axes != 0))
{
std_msgs::String mission_msg;
mission_msg.data = "J3^";
if(joystick_y_axes == 0)
{
mission_msg.data += "-^";
}
else if (joystick_y_axes > 0)
{
mission_msg.data += "u^";
}
else
{
mission_msg.data += "d^";
}
if(joystick_x_axes == 0)
{
mission_msg.data += "-";
}
else if (joystick_x_axes > 0)
{
mission_msg.data += "r";
}
else
{
mission_msg.data += "l";
}
mission_pub_.publish(mission_msg);
last_interaction_time_ = ros::Time::now();
}
}
if(msg->buttons[manual_mode_select_] == 1)
{
if(mission_running_ == true)
{
std_msgs::Empty empty_msg;
cancel_pub_.publish(empty_msg);
}
keyboard_linear_speed_ = 0.0f;
keyboard_angular_speed_ = 0.0f;
manual_locomotion_mode_ = true;
last_interaction_time_ = ros::Time::now();
}
if((manual_locomotion_mode_ == true) &&
(msg->buttons[default_camera_pos_select_] == 1))
{
std_msgs::String mission_msg;
mission_msg.data = "J3^c^-";
mission_pub_.publish(mission_msg);
last_interaction_time_ = ros::Time::now();
}
}
The keyboardCallBack is called when a message is received on the keyboard/keydown topic. The key presses can be used to move the robot around and to move the head/camera when in manual mode.
The data in the message is checked to see if it corresponds to a key that we are interested in.
The number keys are used to select robot missions. Currently, we are only interested in mission 2.
The 'A' key is used to acknowledge a section of a mission by sending a message on the /missions/missions_acknowledge topic.
The 'C' key is used to request that the current mission be cancelled, this is done by sending a message on the /missions/mission_cancel topic.
The 'D' key is used to move the head/camera back to the default position if the robot is in manual locomotion mode.
The 'M' key is used to put the robot in manual locomotion mode. If a mission is currently in progress, a request to cancel the mission is also sent.
The 'R' key is used to reset the robot's odometry values by sending a message on the /command/reset_odometry topic.
The keyboard numeric keypad is used to control movement of the robot when in manual locomotion mode. For example key '1' will result in linear velocity in the reverse direction plus angular velocity in the ant-clockwise direction. The amount of velocity is set by the current values in linear_set_speed_ and angular_set_speed_ variables. These values can be increased or decreased by the use of the '+', '-', '*' and '/' keys on the numeric keypad. The '+' key will increase the robot linear velocity by 10% whilst the '-' key will decrease the linear velocity by 10%. The '*' increases the angular velocity by 10% and the '/' key decreases the angular velocity by 10%.
The space key will stop the robot moving.
The concept of the linear and angular velocity will be discussed when the Twist message is described. But basically the robot does not contain steerable wheels so a change in direction will be achieved by requesting different speeds and or direction of the two motors. The amount of steering required will be set by the angular velocity.
The up/down/left and right keys are used to move the head/camera when in manual mode.
void RodneyNode::keyboardCallBack(const keyboard::Key::ConstPtr& msg)
{
if(((msg->code >= keyboard::Key::KEY_1) && (msg->code <= keyboard::Key::KEY_9)) &&
((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0))
{
std_msgs::String mission_msg;
mission_msg.data = "M" + std::to_string(msg->code-keyboard::Key::KEY_0);
mission_pub_.publish(mission_msg);
mission_running_ = true;
manual_locomotion_mode_ = false;
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_c) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
if(mission_running_ == true)
{
std_msgs::Empty empty_msg;
cancel_pub_.publish(empty_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_a) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
if(mission_running_ == true)
{
std_msgs::Empty empty_msg;
ack_pub_.publish(empty_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_d) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^c^-";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_m) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
if(mission_running_ == true)
{
std_msgs::Empty empty_msg;
cancel_pub_.publish(empty_msg);
}
keyboard_linear_speed_ = 0.0f;
keyboard_angular_speed_ = 0.0f;
manual_locomotion_mode_ = true;
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_r) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
std_msgs::Empty empty_msg;
reset_odom_.publish(empty_msg);
}
else if((msg->code == keyboard::Key::KEY_KP1) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = -linear_set_speed_;
keyboard_angular_speed_ = -angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP2) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = -linear_set_speed_;
keyboard_angular_speed_ = 0.0f;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP3) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = -linear_set_speed_;
keyboard_angular_speed_ = angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP4) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = 0.0f;
keyboard_angular_speed_ = angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP6) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = 0.0f;
keyboard_angular_speed_ = -angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP7) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = linear_set_speed_;
keyboard_angular_speed_ = angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP8) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = linear_set_speed_;
keyboard_angular_speed_ = 0.0f;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP9) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = linear_set_speed_;
keyboard_angular_speed_ = -angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_SPACE)
{
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_= 0.0f;
keyboard_angular_speed_ = 0.0f;
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_KP_PLUS)
{
if(manual_locomotion_mode_ == true)
{
linear_set_speed_ += ((10.0/100.0) * linear_set_speed_);
ROS_INFO("Linear Speed now %f", linear_set_speed_);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_KP_MINUS)
{
if(manual_locomotion_mode_ == true)
{
linear_set_speed_ -= ((10.0/100.0) * linear_set_speed_);
ROS_INFO("Linear Speed now %f", linear_set_speed_);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_KP_MULTIPLY)
{
if(manual_locomotion_mode_ == true)
{
angular_set_speed_ += ((10.0/100.0) * angular_set_speed_);
ROS_INFO("Angular Speed now %f", angular_set_speed_);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_KP_DIVIDE)
{
if(manual_locomotion_mode_ == true)
{
angular_set_speed_ -= ((10.0/100.0) * angular_set_speed_);
ROS_INFO("Angular Speed now %f", angular_set_speed_);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_UP)
{
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^d^-";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_DOWN)
{
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^u^-";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_LEFT)
{
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^-^l";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_RIGHT)
{
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^-^r";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else
{
;
}
}
The batteryCallback function is called when a message is received on the main_battery_status topic. This topic is of message type sensor_msgs/BatteryState which contains numerous battery information. For now, we are just interested in the battery voltage level.
The callback will publish a message which contains an indication of a good or bad level along with the battery voltage level. This is published on the /robot_face/expected_input topic so will be displayed below the robot's animated face.
The level at which the battery is considered low is configurable by using the parameter server. If the voltage is below this value, as well as the warning below the animated face, a request will be sent every 5 minutes requesting that the robot speaks a low battery warning. This request will be sent to the rodney_mission_node with an ID of "J2". The first parameter is the text to speak and the second parameter is the text that the animated face should use for its display. This includes the ":(" smiley so that the robot face looks sad.
void RodneyNode::batteryCallback(const sensor_msgs::BatteryState::ConstPtr& msg)
{
std::stringstream ss;
ss << std::fixed << std::setprecision(2) << msg->voltage;
std::string voltage = ss.str();
std_msgs::String status_msg;
replace(voltage.begin(), voltage.end(), '.', ',');
if(msg->voltage > voltage_level_warning_)
{
status_msg.data = "Battery level OK ";
battery_low_count_ = 0;
}
else
{
if(battery_low_count_ > 1)
{
status_msg.data = "Battery level LOW ";
if((ros::Time::now() - last_battery_warn_).toSec() > (5.0*60.0))
{
last_battery_warn_ = ros::Time::now();
std_msgs::String mission_msg;
mission_msg.data = "J2^battery level low^Battery level low:(";
mission_pub_.publish(mission_msg);
}
}
else
{
battery_low_count_++;
}
}
status_msg.data += voltage + "V";
face_status_pub_.publish(status_msg);
}
The completeCallBack function is called when a message is received on the /missions/mission_complete topic. An indication that the robot is no longer running a mission is set by setting missions_running_ to false.
void RodneyNode::completeCallBack(const std_msgs::String::ConstPtr& msg)
{
mission_running_ = false;
last_interaction_time_ = ros::Time::now();
}
The motorDemandCallBack function is called when a message is received on the demand_vel topic.
The robot movements will be either manual or autonomous, this node is responsible for using either the demands created from the keyboard or joystick in manual mode, or from the autonomous subsystem. This callback simply stores the linear and angular demands from the autonomous subsystem.
void RodneyNode::motorDemandCallBack(const geometry_msgs::Twist::ConstPtr& msg)
{
linear_mission_demand_ = msg->linear.x;
angular_mission_demand_ = msg->angular.z;
}
The sendTwist function is one of those called from main in our loop. It decides which input should be used for requesting the actual electric motor demands, either joystick, keyboard or the autonomous subsystem. The chosen demands are published in a message on the cmd_vel topic. Notice that a demand is always published as its normal practice for the system to keep up a constant rate of demands. If the demands are not sent, then the part of the system controlling the motors can shut them down as a safety precaution.
The message is of type geometry_msgs/Twist and contains two vectors, one for linear velocity (metres/second) and one for angular velocity (radians/second). Each vector gives the velocities in three dimensions, now for linear we will only use the x direction and for angular only the velocity around the z direction. Using this message type may seem like overkill but it does mean that we can make use of existing path planning and obstacle avoidance software later in the project. Publishing this topic also means that we can simulate our robot movements in Gazebo. Gazebo is a robot simulation tool which we will use later in this part of the article to test some of our code.
To ramp the velocities to the target demands the callback function makes use of two helper functions, rampedTwist and rampedVel. We use these to ramp the target velocities in order to stop skidding and shuddering which may occur if we attempted to move the robot in one big step change in velocity. The code in these two helper functions is based on Python code from the O'Reilly book "Programming Robots with ROS".
void RodneyNode::sendTwist(void)
{
geometry_msgs::Twist target_twist;
if(manual_locomotion_mode_ == true)
{
if((keyboard_linear_speed_ == 0) && (keyboard_angular_speed_ == 0))
{
target_twist.linear.x = joystick_linear_speed_;
target_twist.angular.z = joystick_angular_speed_;
}
else
{
target_twist.linear.x = keyboard_linear_speed_;
target_twist.angular.z = keyboard_angular_speed_;
}
}
else
{
target_twist.linear.x = linear_mission_demand_;
target_twist.angular.z = angular_mission_demand_;
}
ros::Time time_now = ros::Time::now();
last_twist_ = rampedTwist(last_twist_, target_twist, last_twist_send_time_, time_now);
last_twist_send_time_ = time_now;
twist_pub_.publish(last_twist_);
}
geometry_msgs::Twist RodneyNode::rampedTwist(geometry_msgs::Twist prev,
geometry_msgs::Twist target,
ros::Time time_prev, ros::Time time_now)
{
geometry_msgs::Twist retVal;
retVal.angular.z = rampedVel(prev.angular.z, target.angular.z,
time_prev, time_now, ramp_for_angular_);
retVal.linear.x = rampedVel(prev.linear.x, target.linear.x,
time_prev, time_now, ramp_for_linear_);
return retVal;
}
float RodneyNode::rampedVel(float velocity_prev, float velocity_target,
ros::Time time_prev, ros::Time time_now,
float ramp_rate)
{
float retVal;
float sign;
float step = ramp_rate * (time_now - time_prev).toSec();
if(velocity_target > velocity_prev)
{
sign = 1.0f;
}
else
{
sign = -1.0f;
}
float error = std::abs(velocity_target - velocity_prev);
if(error < step)
{
retVal = velocity_target;
}
else
{
retVal = velocity_prev + (sign * step);
}
return retVal;
}
The last function checkTimers is the other function called from main in our loop. Now the functionality here serves two purposes. The first is if the robot is inactive, that is that it has not been manually controlled or it finished the last mission more than 15 minutes ago, it will play a pre-existing wav file to remind you that it is still powered up. This functionality can be disabled by use of the /sounds/enabled parameter in the parameter server.
Oh and the second purpose of the functionality I'm afraid is an indication of my sense of humour, all my pre-existing wav files are recordings of Sci-Fi robots. I figured if a robot got bored, it may amuse itself by doing robot impressions! "Danger Will Robinson, danger". Anyway, if you don't like this idea, you can disable the functionality or just play something else to show it is still powered up and inactive.
There are a number of wav file names and text sentences to go with the wav files loaded into the parameter server. When it is time to play a wav file, a random number is generated to select which wav file to play. The request is then sent using the ID "J1".
void RodneyNode::checkTimers(void)
{
if((wav_play_enabled_ == true) && (mission_running_ == false) &&
((ros::Time::now() - last_interaction_time_).toSec() > (15.0*60.0)))
{
last_interaction_time_ = ros::Time::now();
int random = (rand()%wav_file_names_.size())+1;
std_msgs::String mission_msg;
std::string path = ros::package::getPath("rodney");
mission_msg.data = "J1^" + path + "/sounds/" +
wav_file_names_[std::to_string(random)] +
"^" + wav_file_texts_[std::to_string(random)];
mission_pub_.publish(mission_msg);
}
}
Joystick Node
Now throughout this article, we have added functionality for the robot to be moved manually by using a joystick/game pad controller. There is a joystick node available on the ROS Wiki website called joy.
However, I tried this package on two different Linux PCs and found that I kept getting segmentation faults. Instead of doing any deep investigation to see what the problem was, I wrote my own simple joystick node. It's simpler than the one on the ROS website as I don't bother with worrying about sticky buttons, etc.
I would suggest that you try and use the package from the ROS website but if you have similar problems, then you can use my ROS package which is available in the joystick folder. I have used it successfully with a Microsoft Xbox 360 Wired Controller and the joystick_node.cpp file is reproduced below:
#include <joystick/joystick_node.h>
#include <fcntl.h>
#include <stdio.h>
#include <linux/joystick.h>
Joystick::Joystick(ros::NodeHandle n, std::string device)
{
nh_ = n;
joy_status_pub_ = nh_.advertise<sensor_msgs::Joy>("joy", 5);
js_ = open(device.c_str(), O_RDONLY);
if (js_ == -1)
{
ROS_ERROR("Problem opening joystick device");
}
else
{
int buttons = getButtonCount();
int axes = getAxisCount();
joyMsgs_.buttons.resize(buttons);
joyMsgs_.axes.resize(axes);
ROS_INFO("Joystick number of buttons %d, number of axis %d", buttons, axes);
}
}
void Joystick::process(void)
{
js_event event;
FD_ZERO(&set_);
FD_SET(js_, &set_);
tv_.tv_sec = 0;
tv_.tv_usec = 250000;
int selectResult = select(js_+1, &set_, NULL, NULL, &tv_);
if(selectResult == -1)
{
ROS_ERROR("Error with select joystick call"); }
else if (selectResult)
{
if(read(js_, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
{
;
}
else
{
switch (event.type)
{
case JS_EVENT_BUTTON:
case JS_EVENT_BUTTON | JS_EVENT_INIT:
joyMsgs_.buttons[event.number] = (event.value ? 1 : 0);
time_last_msg_ = ros::Time::now();
joyMsgs_.header.stamp = time_last_msg_;
joy_status_pub_.publish(joyMsgs_);
break;
case JS_EVENT_AXIS:
case JS_EVENT_AXIS | JS_EVENT_INIT:
joyMsgs_.axes[event.number] = event.value;
if((ros::Time::now() - time_last_msg_).toSec() > 0.1f)
{
time_last_msg_ = ros::Time::now();
joyMsgs_.header.stamp = time_last_msg_;
joy_status_pub_.publish(joyMsgs_);
}
default:
break;
}
}
}
else
{
time_last_msg_ = ros::Time::now();
joyMsgs_.header.stamp = time_last_msg_;
joy_status_pub_.publish(joyMsgs_);
}
}
int Joystick::getButtonCount(void)
{
int buttons;
if (ioctl(js_, JSIOCGBUTTONS, &buttons) == -1)
{
buttons = 0;
}
return buttons;
}
int Joystick::getAxisCount(void)
{
int axes;
if (ioctl(js_, JSIOCGAXES, &axes) == -1)
{
axes = 0;
}
return axes;
}
int main(int argc, char **argv)
{
std::string device;
ros::init(argc, argv, "joystick_node");
if (argc > 1)
{
device = argv[1];
}
else
{
device = "/dev/input/js0";
}
ros::NodeHandle n;
Joystick joystick_node(n, device);
std::string node_name = ros::this_node::getName();
ROS_INFO("%s started", node_name.c_str());
while(ros::ok())
{
joystick_node.process();
ros::spinOnce();
}
return 0;
}
Using the Code
To test the code we have developed so far, I'm going to run some tests on the actual robot hardware but we can also run some tests on the Gazebo robot simulator tool running on a Linux PC. In the folder rodney/urdf, there is a file called rodney.urdf which models the Rodney Robot. How to write a URDF (Unified Robot Description Format) model would require many articles itself but as always there is information on the ROS Wiki website about URDF. My model is nowhere near perfect and needs some work, but we can use it here to test the robot locomotion. All the files to do this are included in the rodney folder and the rodney_sim_control folder.
Building the ROS Packages on the Workstation
On the workstation as well as running the simulation, we also want to run the keyboard and joystick nodes so that we can control the actual robot hardware remotely.
Create a workspace with the following commands:
$ mkdir -p ~/test_ws/src
$ cd ~/test_ws/
$ catkin_make
Copy the packages rodney, joystick, rodney_sim_control and ros-keyboard (from https://github.com/lrse/ros-keyboard) into the ~/test_ws/src folder and then build the code with the following commands:
$ cd ~/test_ws/
$ catkin_make
Check that the build completes without any errors.
Running the Simulation
In the rodney_sim_control package, there is a launch file that will load the robot model into the parameter server, launch Gazebo and spawn a simulation of the robot. Launch this file with the following commands:
$ cd ~/test_ws/
$ source devel/setup.bash
$ roslaunch rodney_sim_control rodney_sim_control.launch
After a short time, you should see the model of Rodney in an empty world. The simulation is currently paused.
In a new terminal, load the rodney config file and run the rodney node with the following commands:
$ cd ~/test_ws
$ source devel/setup.bash
$ rosparam load src/rodney/config/config.yaml
$ rosrun rodney rodney_node
An info message should be seen reporting that the node is running.
The first test is going to test that a message on the demand_vel topic, as if from the autonomous subsystem, will control the robot's movements.
In Gazebo, click the play button, bottom left of the main screen, to start the simulation. In a new terminal, type the following to send a message on the demand_vel topic.
$ rostopic pub -1 /demand_vel geometry_msgs/Twist '{linear: {x: 0.5}}'
The simulated robot will move forward at a velocity of 0.5 metres/second. Reverse the direction with the following command:
$ rostopic pub -1 /demand_vel geometry_msgs/Twist '{linear: {x: -0.5}}'
You can stop the robot movement with the following command:
$ rostopic pub -1 /demand_vel geometry_msgs/Twist '{linear: {x: 0.0}}'
Next make the simulated robot turn on the spot with the following command:
$ rostopic pub -1 /demand_vel geometry_msgs/Twist '{angular: {z: 1.0}}'
Repeating the command with a negative value will cause the robot to rotate clockwise and then stop the movement with a value of zero.
Next, we will test the movement with the keyboard functionality.
$ cd ~/test_ws
$ source devel/setup.bash
$ rosrun keyboard keyboard
A small window whose title is "ROS keyboard input" should be running. Make sure this window has the focus and then press 'm' key to put the robot in manual locomotion mode.
Ensure "num lock" is not selected.
You can now use the keyboards numeric keypad to drive the robot around the simulated world. The following keys can be used to move the robot.
Key pad 8 - forward
Key pad 2 - reverse
Key pad 4 - rotate anti-clockwise
Key pad 6 - rotate clockwise
Key pad 7 - forward and left
Key pad 9 - forward and right
Key pad 1 - reverse and left
Key pad 3 - reverse and right
Key pad + increase the linear velocity
Key pad - decrease the linear velocity
Key pad * increase the angular velocity
Key pad / decrease the angular velocity
The space bar will stop the robot.
Next we can test the movement with the joystick controller. Ensure the robot is stationary. In a new terminal, issue the following commands:
$ cd ~/test_ws/
$ source devel/setup.bash
$ rosrun joystick joystick_node
A message showing the node has started should be displayed. With the configuration given in an unchanged rodney/config/config.yaml file and a wired Xbox 360 controller, you can control the simulated robot with the controls shown in the image below:
From the Gazebo menu, other objects can be inserted into the world. The video below shows the movement test running using Gazebo. Note that in the video Rodney is a 4 wheel drive robot, I have since updated the model and the actual robot has 2 wheel drive and casters. This will all be explained in the next article when we move the real robot hardware.
Building the ROS Packages on the Pi (Robot Hardware)
If not already done, create a catkin workspace on the Raspberry Pi and initialise it with the following commands:
$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make
Copy the packages face_recognition, face_recognition_msgs, head_control, pan_tilt, rondey, rodney_missions, servo_msgs, speech and ros-keyboard (from https://github.com/lrse/ros-keyboard) into the ~/rodney_ws/src folder.
Unless you want to connect the joystick controller directly to the robot, you don't need to build the joystick package on the robot hardware. You do however need to build the keyboard package as it includes a message unique to that package. I'm going to be using the Linux PC connected to the same network as the robot to control it remotely.
Build the code with the following commands:
$ cd ~/rodney_ws/
$ catkin_make
Check that the build completes without any errors.
You will also need to compile and download the Arduino code to the Nano to control the servos.
If not already done, you will need to train the face recognition software, see Part 2.
Running the Code on the Robot
Now we are ready to run our code. With the Arduino connected to a USB port, use the launch file to start the nodes with the following commands. If no master node is running in a system, the launch command will also launch the master node, roscore:
$ cd ~/rodney_ws
$ source devel/setup.bash
$ roslaunch rodney rodney.launch
On the workstation, run the following commands to start the keyboard node:
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun keyboard keyboard
A small window whose title is "ROS keyboard input" should be running.
The first test we will run on the robot hardware is "Mission 2". Make sure keyboard window has the focus and then press '2' key to start the mission.
The robot should start moving the head/camera scanning the room for known faces. Once it has completed the scan within its head movement range, it will either report that no one was recognised or a greeting to those it did recognise.
The next test will check the ability to move the head/camera in manual mode using the keyboard. Make sure keyboard window has the focus and then press 'm' to put the system in manual mode. Used the cursor keys to move the head/camera. Press the 'd' key to return the head/camera to the default position.
The next test will check the ability to move the head/camera in manual mode using the joystick controller. In a new terminal on the workstation, type the following commands:
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun joystick joystick_node
A message showing the node has started should be displayed. With the configuration given in an unchanged rodney/config/config.yaml file and a wired Xbox 360 controller, you can control the robot head/camera movement with the controls shown in the image below:
For the next test, we will test the status indication. In a terminal at the workstation, type the following commands:
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rostopic pub -1 main_battery_status sensor_msgs/BatteryState '{voltage: 12}'
The status below the robot face should read "Battery level OK 12,00V".
In the terminal, issue the following command:
$ rostopic pub -1 main_battery_status sensor_msgs/BatteryState '{voltage: 9.4}'
The status below the robot face should read "9,40V".
In the terminal, issue the following command twice more:
$ rostopic pub -1 main_battery_status sensor_msgs/BatteryState '{voltage: 9.4}'
The status below the robot face should read "Battery level low 9,40V", the robot should speak a battery low warning and the facial expression should be sad.
Send the message again within 5 minutes of the last message. The warning should not be spoken.
Wait for 5 minutes and send the message again. This time, the spoken warning should be repeated.
The next test will check the functionality for wav file playback. 15 minutes after issuing the last command from either the keyboard or joystick node, the robot should play a random wav file and animate the mouth along with the wav file.
To aid debugging, here is an output from rqt_graph of the current system. A full size copy of the image is included in the source zip file.
Points of Interest
In this part of the article, we have added code to control the robot actions and brought the code for Design Goal 1 and 2 together to form mission 2.
In the next article, we will complete Design Goal 3 by adding motors, a motor controller board and software to drive the board. We will also discuss all the robot hardware required to build Rodney including circuit diagrams and a list of the hardware required.
History
- 13th November, 2018: Initial release
- 13th November, 2018: Version 2: Fixed embedded video
- 6th December, 2018: Version 3: Fixed bug in rodney_node.cpp. Joystick linear and angular speeds must be set to 0.0 in constructor, otherwise false velocity values in
cmd_vel are sent if no joystick node running - Version 4: Now using radians instead of degrees to conform with the ROS standard and using
SimpleActionStates to control the head movement and face recognition processes
