###################################################################### Control Ned/Ned2 with a Keyboard or a Joystick through a ROS Node ###################################################################### **V.1.1** .. image:: /images/joystick_keyboard/keyboard_joystick.png :alt: Control Ned with keyboard or joystick :width: 100% :align: center 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 :ref:`source/tutorials/setup_ned_ros_multi_machines:Setup ROS multi-machines for Ned/Ned2` tutorial). - Be able to connect via ssh to Ned/Ned2 (for more information, follow the :ref:`source/tutorials/setup_connect_ned_ssh: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: .. code-block:: bash ssh niryo@10.10.10.10 Or through a static IP address, for instance: .. code-block:: bash 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: .. code-block:: python #!/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. .. code-block:: python 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. .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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. .. code-block:: python # 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. .. code-block:: python # [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!