Control Ned/Ned2 with a Keyboard or a Joystick through a ROS Node
V.1.1
Difficulty: medium
Time: ~25 min
Note
- This tutorial is working from:
- The version v3.2.0 of the ned_ros_stack
The goal of this tutorial is to create a Python ROS node that will listen to the keyboard or to a Joystick Controller and move Ned/Ned2’s arm accordingly. What we’ll explain in this tutorial:
Setup: Quick reminder of ROS Multiple Machine setup
Introduction to the Jog Interface: How to implement a quick demonstration using the jog controller
Use the jog controller to control Ned with the keyboard
Use the jog controller to control Ned with a joystick controller
Control Ned/Ned2 directly through the Hardware Interface.
Requirements
In order to follow this tutorial, you will have to:
Use Ned/Ned2 and a computer.
Have a Joystick controller if you want to control Ned/Ned2 with it.
Set up the ROS system to work across Ned/Ned2 and your computer, in a multiple machines system (for more information, follow the Setup ROS multi-machines for Ned/Ned2 tutorial).
Be able to connect via ssh to Ned/Ned2 (for more information, follow the Connect to Ned/Ned2 via SSH tutorial).
The setup part will explain how to connect in MultiMachines and how to connect to Ned/Ned2 via SSH, but if you are facing issues, please refer to the mentioned tutorials.
Note
In this tutorial, we explain briefly the codes used to control Ned with a ROS node. Those codes are in a package ros_node_control, that you can find on our github repository: here.
Setup
SSH connection:
First, start Ned/Ned2 and make sure your computer can connect to it through ssh. In a terminal, you can connect through hotspot with:
ssh niryo@10.10.10.10
Or through a static IP address, for instance:
ssh niryo@192.168.1.52
Once you are connected, stop the current background execution of the ROS stack with:
sudo service niryo_robot_ros stop
Multi Machines setup:
Now, setup your ROS_MASTER_URI and ROS_IP environment variables on both your computer and Ned/Ned2. The master machine will be Ned/Ned2. Let’s assume you are connected to Ned/Ned2 with its static IP address, which is 192.168.1.52.
On your computer, you can check the current values of those variables with:
printenv | grep ROS_MASTER_URI printenv | grep ROS_IP
You’ll have to define the ROS_MASTER_URI variable so that it refers to Ned IP address, as:
export ROS_MASTER_URI=http://192.168.1.52:11311
You also have to define your computer IP address, (that you can find with the ifconfig command line) as:
export ROS_IP=192.168.1.7
On Ned/Ned2 through SSH, you can also check the current values of those variables with:
printenv | grep ROS_MASTER_URI printenv | grep ROS_IP
Define the ROS_MASTER_URI:
export ROS_MASTER_URI=http://192.168.1.52:11311
Define the ROS_IP:
export ROS_IP=192.168.1.52
You can then start a new roscore by launching the Ned/Ned2 ROS stack on the robot (in a terminal connected to Ned/Ned2 via ssh):
roslaunch niryo_robot_bringup niryo_ned_robot.launch
Now, you can subscribe and publish to topics running on Ned/Ned2 with your computer.
Note
You can check that the Multimachine setup is well working with the following command on your computer:
rostopic echo /niryo_robot/learning_mode/state
You’ll see the current learning mode state, being updated regularly.
Use basics functionalities of the Jog Interface with a ROS Node
The Jog Interface uses the jog_controller class (from the niryo_robot_commander package), that allows to move the arm given a desired shifting value, without using motion planning.
First, we create a node that simply uses the Jog Interface, and execute the same movements in a loop. On your computer, create a catkin workspace, put our package in the /src (find the package on our github) and do a
catkin_make
source devel/setup.bash
We’ll use here a python ROS Node named ‘control_node_jog.py’. To launch it, use:
rosrun ros_node_control control_node_jog.py
Code explanation :
Imports:
Here are the imports lines we need:
#!/usr/bin/env python
import rospy
from niryo_robot_msgs.msg import CommandStatus
from std_msgs.msg import Bool
from niryo_robot_commander.srv import JogShift, JogShiftRequest
from niryo_robot_msgs.srv import SetBool
Class definition:
We use a class named ‘JogClient’, that will, among other things, use the ‘/niryo_robot/jog_interface/jog_shift_commander’ service.
class JogClient:
def __init__(self):
rospy.init_node('jog_client_node', anonymous=True)
self.__jog_enabled = False
self.__jog_enabled_subscriber = rospy.Subscriber('/niryo_robot/jog_interface/is_enabled',
Bool, self.__callback_subscriber_jog_enabled,
queue_size=1)
self.current_learning_mode = True
rospy.Subscriber('/niryo_robot/learning_mode/state', Bool, self.update_learning_state)
self.check_calibration()
if self.current_learning_mode == True:
self.set_learning_mode(False)
Note
In order to use the services and functions provided by the Jog Controller to move the arm, the learning mode must be deactivated, otherwise it won’t work.
Here, we first call the ‘check_calibration’ function, that:
Check if a motor calibration is needed, thanks to the ‘niryo_robot_hardware_interface’ package.
If a calibration is needed, use the service ‘/niryo_robot/joints_interface/calibrate_motors’ to launch an automatic calibration.
Check if calibration is done and succeeded.
Then, we deactivate the learning mode, using the ‘/niryo_robot/learning_mode/activate’ service with the ‘set_learning_mode’ function.
The JogClient class also implements other functions, that we’ll detail later. Here is the list of functions implemented in the JogClient Class:
__update_learning_state: update the current learning mode state.
__callback_subscriber_jog_enabled: update the current jog interface state (enabled or not).
set_learning_mode: change the current state of the learning mode.
set_jog: change the current state of the jog interface.
check_calibration: check if calibration is needed and launch it.
ask_for_jog_shift: use the ‘/niryo_robot/jog_interface/jog_shift_commander’ service to move the arm according to the command and the shifting value.
Main program:
In the main function of the python file, we create a JogClient object that we’ll use to move the arm. Here, the code allows us to:
Apply a positive and a negative shift on the 5th joint.
Apply a positive and a negative shift on pose (uses IK solver) (on y and z axis).
Note that the shifting values are small on purpose because you should not apply a huge differential when using the Jog Interface to move the arm, especially when it applies to pose.
When shifting pose, after each call to the jog_shift_commander service (through the ‘ask_for_jog_shift’ function), we check that the status of the response is not -231. Indeed, a -231 status mean that the IK solver was not able to find a solution for the desired pose. To avoid further movements (potentially undesirable positions), we then stop the current execution.
if __name__ == "__main__":
# Creating Client Object
jc = JogClient()
jc.set_jog(True)
for sign in [1, -1]:
# this 'stop' boolean is used so that if one jog shift is not doable, instead of doing the
# next one, it abord the loop. Indeed, doing some jog shifts (with this sign method we use) when some were skipped
# can lead the robot to strange position.
for i in range(10):
print('joints jog', i)
response = jc.ask_for_jog_shift(cmd=JogShiftRequest.JOINTS_SHIFT,
shift_values=[0.0, 0.0, 0.0, 0.0, sign * 0.05, 0.0])
for sign in [1, -1]:
stop = False
for i in range(10):
print('pose jog', i)
response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
shift_values=[0.0,sign * 0.01, sign * 0.01, 0.0, 0.0, 0.0])
if response.status == -231:
stop = True
break
if stop == True:
break
jc.set_learning_mode(True)
Control Ned/Ned2 with the Keyboard
We will now use the Jog Controller to control Ned/Ned2 with a keyboard, thanks to the control_with_keyboard.py node.
We use the python package pynput, that you can install with:
pip install pynput
Pynput allows us to monitor the keyboard and retrieve pressed key to process actions. To launch the node, launch:
rosrun ros_node_control control_with_keyboard.py
Code explanation:
We re-use the python class ‘JogClient’ of the previous code. In the main function, we use a keyboard listener with de Pynput library, that will execute in a thread so it’s non-blocking:
listener = keyboard.Listener(
on_press=on_press,
on_release=on_release)
listener.start()
And we define on_press and on_release functions, which will be called everytime a key is pressed and released, as:
def on_press(key):
global current_key
try:
current_key=key.char
except AttributeError:
print('special key {0} pressed'.format(
key))
current_key=''
def on_release(key):
global current_key
current_key = ''
if key == keyboard.Key.esc:
return False
if key == keyboard.Key.ctrl:
return False
else :
current_key = ''
The current_key variable is initialized in the main function as “current_key = ‘’”, and we use it as a global variable in the functions of the keyboard thread so both threads can access and modify the same variable.
Moreover, when the ‘esc’ key or the ‘ctrl’ key are pressed and released, the program will end.
Finally, in the main function, we use a while loop, to do action according to the current pressed key:
while(listener.is_alive()):
while(current_key != ''):
# SHIFT POSE
if current_key == 'i': #i : z axis (+)
response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
shift_values=[0.0,0.0, 0.01, 0.0, 0.0, 0.0])
elif current_key == 'k': #k : z axis (-)
response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
shift_values=[0.0,0.0, -0.01, 0.0, 0.0, 0.0])
elif current_key == 'l': #l : y axis (+)
response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
shift_values=[0.0,0.01, 0.0, 0.0, 0.0, 0.0])
elif current_key == 'j': #j : y axis (-)
response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,
shift_values=[0.0, -0.01, 0.0, 0.0, 0.0, 0.0])
# The rest of the python code is not included there, but explained right after
The arm can be controlled with the keyboard with:
Pose of the end effector:
i / k : shift of 0.01 (goes up) / - 0.01 (goes down) along the z axis
l / j : shift of 0.01 (goes right) / - 0.01 (goes left) along the y axis
p / m : shift of 0.01 (goes frontward) / - 0.01 (goes backward) along the x axis
Note
In this code, we didn’t implement the shift over Roll, Pitch and Yaw, because the Jog Controller used to shift a Pose uses the IK processing to reach a certain pose. Because of that, movements can be really sudden when the arm tries to go from its current Pitch pose to the next one. However, this feature is implemented in the section ‘Control Ned through the Hardware Interface’, as an example.
Joints:
a / q : shift the 1st joint of 0.05 / - 0.05
z / s : shift the 2nd joint of 0.05 / - 0.05
e / d : shift the 3rd joint of 0.05 / - 0.05
r / f : shift the 4th joint of 0.05 / - 0.05
t / g : shift the 5th joint of 0.05 / - 0.05
y / h : shift the 6th joint of 0.05 / - 0.05
Now you can control Ned/Ned2 through simulation and in reality with your keyboard!
Control Ned/Ned2 with a joystick controller
Setup joystick
For this part, you’ll need to have a joystick controller that you can plug to your computer, through USB for instance. Also, we use the joystick drivers ROS package, that you can install with:
sudo apt install ros-melodic-joystick-drivers
First, plug your joystick to your computer. Then, you can use the joystick driver by launching in a terminal:
rosrun joy joy_node
If your joystick input is not found, you can check which inputs are used with:
ls /dev/input/
And you can then change the input with:
rosparam set joy_node/dev "/dev/input/js0"
Note
Each terminal you work in on your computer should have the same ROS_MASTER_URI (referencing the robot IP address) and ROS_IP (referencing your computer’s IP address). To check their value, do a:
printenv | grep ROS
You can now check that you can read the joystick output with:
rostopic echo /joy
When you press or unpress a button, a message of type “sensor_msgs/Joy” will be published here.
Now, you should have:
The roscore running on Ned/Ned2.
A terminal, linked in MultiMachines to Ned/Ned2, which runs the joy_node node.
In a third terminal, also linked in MultiMachines with Ned/Ned2, launch the node that will gather information from the /joy topic, process them and control Ned/Ned2 :
rosrun ros_node_control control_with_joystick.py
You should now be able to control Ned/Ned2 with the joystick:
Note
For this tutorial, we used a Xbox joystick. If you want to use another joystick, you may have to change the ROS node code to adapt it to your joystick, according to what you want Ned/Ned2 to do with each button!
How to use:
A button: activate Learning Mode.
B button: deactivate Learning Mode.
Left joystick:
Up/Down: Positive / Negative translation of effector along the X axis.
Directional arrow keys:
Right / Left: Positive / Negative translation of effector along the Y axis.
Up / Down: Positive / Negative translation of effector along the Z axis.
Right Joystick:
Right / Left : Positive / Negative shift of the 5th joint.
Up / Down : Positive / Negative shift of the 6th joint.
Code explanation - Joystick
We use again the class JogClient, but we modified it in order to subscribe to the /joy topic and process its messages. According to what was received on this topic, we update the class attribute self.shift_values, that is used by a thread launched at the initialization of the node, working with the move_arm function:
def move_arm(self):
while(1):
try :
i=0
if self.zAxis != 0 or self.yAxis != 0 or abs(self.xAxis) == 1.0 or abs(self.joint5) == 1.0 or abs(self.joint6) == 1.0:
if self.current_mode == "Pose" :
if i<3: #just to make sure we don't ask for too many jog shift at a time
response = jc.ask_for_jog_shift(cmd=JogShiftRequest.POSE_SHIFT,shift_values=self.shift_values)
i+=1
elif self.current_mode == "Joint":
if i<3: #just to make sure we don't ask for too many jog shift at a time
response = jc.ask_for_jog_shift(cmd=JogShiftRequest.JOINTS_SHIFT,shift_values=self.shift_values)
i+=1
except KeyboardInterrupt:
print('quit')
self.thread_move.join()
break
This function sends a comand to the jog_controller of Ned/Ned2 according to the current desired shift_value.
Customize the node:
If you want to change what Ned/Ned2 does when buttons or joystick are used, refer to what you see when you launch:
rostopic echo /joy
You can use different buttons and see which value is changed in buttons and axes field of the message. As instance, the A button is the first element of the buttons list. You can retrieve its state with:
def joy_callback(self,data):
listbutton = data.buttons
self.buttonA = listbutton[0]
You can then check for the state of this button: if it is pressed, it will be equal to 1.0, and if it is not pressed, it will be 0.0. According to that, you can choose what command you’ll send to Ned/Ned2!
Control Ned/Ned2 through the hardware interface
In the previous parts, we used the Robot Commander package. Here, we’ll control Ned/Ned2 directly through the Hardware Interface Package, with the keyboard. We use the Joint Trajectory Controller through the topic /niryo_robot_follow_joint_trajectory_controller/command.
Note
You still need to be connected to Ned/Ned2 in MultiMachines. See the Setup section for more information.
The node used to control Ned/Ned2 here is implemented in the file control_with_joint_traj_controller.py in the ros_node_control package.
We use the python package pynput, that you can install with:
pip install pynput
How to use - Control hardware
Once connected to Ned/Ned2 in MultiMachines, launch the node on your computer with:
rosrun ros_node_control control_with_joint_traj_controller.py
You can now control the pose of Ned/Ned2’s end effector with:
i / k : shift of 0.01 (goes up) / - 0.01 (goes down) along the z axis
l / j : shift of 0.01 (goes right) / - 0.01 (goes left) along the y axis
p / m : shift of 0.01 (goes frontward) / - 0.01 (goes backward) along the x axis
a / q : shift roll of 0.05 / -0.05
z / s : shift pitch of 0.05 / -0.05
e / d : shift yaw of 0.05 / -0.05
Code explanation - Control hardware
Node initialization
We listen to the keyboard input in a thread launched in the initialization of the node. We also check if a calibration is needed, and the learning mode is deactivated.
Main loop
If a key is pressed, the target pose of the end effector is updated according to the key and the current effector pose.
Note
In order to get the current end effector pose, we use the lookupTransform from the tf ROS package.
# main loop
while(listener.is_alive()):
# When a key is pressed
if(self.current_key != ''):
target_pose = self.get_current_pose()
if self.current_key in self.key_list:
execute = True
if self.current_key == 'i':
target_pose.pose.position.z +=0.01
elif self.current_key == 'k':
target_pose.pose.position.z -=0.01
elif self.current_key == 'l':
target_pose.pose.position.y +=0.01
elif self.current_key == 'j':
target_pose.pose.position.y -=0.01
# [...]
The list key_list gathers all the above-mentioned keys.
Once the target_pose is updated, we pass it to the get_position_ik function. This function compute the inverse kinematics used to go from the current pose to the target pose, using the /compute_ik service, provided by MoveIt!. This function return the joint trajectory position solution we’ll have to apply to Ned/Ned2’s joints in order to reach the target pose.
If the target pose is reachable (the IK calculations succeeded), we then send the solution found to the send_joint_trajectory function. From the solution, this function create a JointTrajectory message and send it to the /niryo_robot_follow_joint_trajectory_controller/command topic.
# [In the main loop]
if execute==True:
solution, result = self.get_position_ik(target_pose)
if result == True :
self.send_joint_trajectory(solution)
rospy.sleep(0.05) # To avoid an issue regarding trajectory time
elif result == False :
print "IK calculation impossible"
rospy.sleep(0.05)
execute=False
The hardware interface subscribe to this topic and apply the command to the joints controller.
You can now control Ned/Ned2’s end effector directly through its hardware interface!