ROS (Robotic Operating System) - Ver. 1
									(Prof. Fischetti P.)
								Prerequisiti: Linux Ubuntu + Python Base. 
Si fara' riferimento al minicorso:
[1]https://www.youtube.com/watch?v=4aUp2703FFY
e al sito:[2]https://app.theconstruct.ai che contiene un ambiente Linux gratuito di max 2GB
con il Framework ROS che puo essere scelto (fra noetic, kinetic, ...) gia installato.
in [1] viene spiegato come installare ROS su Linux.

######################## ROS Tutorial 1 (ROS1) - Intro - Install and Setup ROS Noetic
*impostare l'ambiente
impostiamo il bash per l'area globale:
echo "source /opt/ros/noetic/setup.bash">> ~/.bashrc
e il bash dell'area di lavoro:
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
E' importante controllare che le preceneti righe siano presenti nel file ~/.bashrc


########## ROS Tutorial 2 (ROS1) - Understand What is a ROS Node
In ROS (versione 1 e non nella versione 2)
Occorre avviare il programma ( o meglio il nodo) master (una sola istanza) con il comando:
$ roscore

I programmi per ROS sono chiamati nodi.
con il comando rosrun si avvia un nodo appartenente ad un determinato package, come ad esempio:
Avvia un nodo (chiamato talker) che scrive, appartenente al package rospy_tutorials
$ rosrun rospy_tutorials talker

Avvia un nodo (chiamato listener) che legge quanto scritto dal talker, appartenente al package rospy_tutorials
$ rosrun rospy_tutorials listener

per visualizzare i nodi in modo grafico: 
$ rqt_graph

In questo esempio si eseguono nodi appartenenti al package turtlesim che simula un robot tartaruga:

$ rosrun turtlesim turtlesim_node
(si puo usare l'abbreviazione rosrun turtlesim <TAB> <TAB>)

Per controllare la tartaruga con latastiera:
$ rosrun turtlesim turtle_teleop_key

######ROS Tutorial 3 (ROS1) - Create and Setup a Catkin Workspace
Per creare un nodo ROS occorre crare (se non esiste) prima una area di lavoro che lo contiene:
questa area di lavoro si usera' per compilare i progetti con l'utility catkin:

Quindi se non esiste (controllare) creiamo una cartella nella home:
$ mkdir catkin_ws
$ cd catkin_ws
creiamo qui una dir:
$ mkdir src

spostandoci nella dir catkin_ws:
$ catkin_make
questo comando crea le cartelle devel e build

$ cd devel
$ ls
ho un'ulteriore file:setup.bash

torniamo nella home:
£ cd

$ source ~/catkin_ws/devel/setup_bash
controllare con un editor il file ~/.bashrc.
se non c'e' aggiungere source ~/catkin_ws/devel/setup_bash al file ~/.bashrc

Creaiamo un nodo nel ws
guardiamo nella cartella
$ cd catkin_ws/src
$ ls
c'e' un file: CMakelists.txt ,Creato da catkin make, che contiene istruzioni per la creazione dei package

############## ROS Tutorial 4 (ROS1) - Create a ROS Package
Per creare un package ROS ad esempio:
$ catkin_create_pkg my_robot_controller

un nodo puo' far riferimento ad altri nodi e librerie:
$ catkin_create_pkg my_robot_controller rospy turtlesim (dopo il nome del pkg ci sono eventuali dipendenze)

viene creata automaticamente una dir con il nome del package my_robot_controller
$ cd my_robot_controller

$ ls
CMakelists.txt package.xml src

Se si desidera installare un editor sofisticato come VS code:
$ sudo snap install code --classic
~/catkin_ws/src$ code .

Ogni pcchetto avra' un file package.xml affiancato da CMakelists.txt

nel file package.xml metteremo le nostre dipendenze.

Ora torno nel mio ws e compilo il mio package

~/catkin_ws$ catkin_make

~/catkin_ws$ cd build
ls
cd ..
~/catkin_ws$ 

dato che il nodo lo scriviamo in Python non c'e bisogno di ricompilarlo dato che e' interpretato,
inoltre il nostro nodo Python utilizzera' la libreria da rospy

Scriviamo quindi un nodo 'ciao mondo'

$ cd catkin_ws/src/my_robot_controller
ls
CMakeLists.txt   package.xml   src

##################### ROS Tutorial 5 (ROS1) - Write a ROS Node with Python
Creiamo una cartella scripts per i miei programmi python:
catkin_ws/src/my_robot_controller$ mkdir scripts
cd scripts
catkin_ws/src/my_robot_controller/scripts$ touch my_first_node.py

rendiamolo eseguibile
$chmod +x my_first_node.py

Scriviamo il nostro script (nel caso [2] si usera' l'editor predefinito vim):
## my_first_node.py
#!/usr/bin/env python3
import rospy

if __name__=='__main__':
	rospy.init_node("test_node")
	rospy.loginfo("Hello from test node")
	rospy.logwarn("This is a warning")
	rospy.logerr("This is an error")
	
	rospy.sleep(1.0)
	
	rospy.loginfo("End of program")
	
Eseguiamolo:
~/catkin_ws/src/my_robot_controller/scripts$ python3 my_first_node.py	

Ma se non avvio prima il nodo master con roscore avro' l'errore di nodo master assente
	
pero' prima controlliamo che l'ambiente sia ok in ~/.bashrc

cioe' in fondo al file ci deve essere sia l'impostazione globale che quella della ws catkin:
....
source /opt/ros/noetic/setup.bash
source ~/catkin_ws/devel/setup.bash

quindi rieseguiamo;
~/catkin_ws/src/my_robot_controller/scripts$ python3 my_first_node.py	

oppure:
~/catkin_ws/src/my_robot_controller/scripts$ ./my_first_node.py	

ora:
$ cd
$ rosrun
devo dare il nome del pacchetto e poi il nome dell'eseguibile cioe':
$ rosrun my_robot_controller my_first_node.py

avro' lo stesso out di prima.

Ora vogliamo stampare un msg 10 volte al secondo:

## my_first_node.py
#!/usr/bin/env python3
import rospy

if __name__=='__main__':
	rospy.init_node("test_node")
	rospy.loginfo("Test node has been started.")
	rate=rospy.Rate(10)#10Hz
	
	while not rosy.is_shutdown():#finche non do ^C
		rospy.loginfo("Hello")
		rate.sleep()
		
Rieseguiamo senza necessita' di ricompilare	
$ rosrun my_robot_controller my_first_node.py

Per vedere i nodi attivi:
$ rosnode list		
/rosout
/test_node

se uccido test_node non lo vedo piu'
uccido con ^C
oppure:
$rosnode kill /test_node


rqt_graph (spunto su debug)

in ROS non si possono avere due nodi con lo stesso nome.


################ROS Tutorial 6 (ROS1) - Understand What is a ROS Topic
Quindi per avviare nodi:
$rosrun rospy_tutorials talker

in un altra finestra di terminale:
$ rosrun rospy_tutorials listener

in unaltra finestra
$rqt_graph

Per vedere la lista dei topic attivi:
$ rostopic list
/chatter
/rosout
/rosout_agg

Vediamo in dettaglio il topic /chatter:
$ rostopic info /chatter
Type: std_msgs/String
Cioe' vedo il tipo di dati (string) che vengono scambiati
???seguito dall'elenco dei subscriber e dei publishers

Vediamo il dettaglio del mssaggio
$ rosmsg show std_msgs/String
string data

quindi questo tipo di messaggio contiene una stringa

Se voglio controllare cosa viene inviato su un certo topic:
$ rostopic echo chatter
data: "hello world 3423453.23345"
....

usiamo ora il package turtlesim:
$ rosrun turtlesim turtlesim_node

in un altra finestra:
$ rosrun turtlesim draw_square

viene disegnato dalla tartaruga un quadrato

Controlliamo graficamente:
$ rqt_graph

Vediamo la lista dei topic
$rostopic list
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

Analizziamo il topic /turtle1(pos
$ rostopic info /turtle1/pose
Per vedere la struttura di un tipo:
$rosmsg show turtlesim/Pose
float32 xz
float32 y
float32 linear_velocity
float32 angular_velocity

Vediamo cosa passa:
$ rostopic echo /turtle1/pose
In deinitiva posso avere piu editor che pubblicano e + lettori che ascoltatno su un argomento
questo meccanismo e' anonimo cioe' un lettore non sa chi sta messaggiando su un argomento

##############ROS Tutorial 7 (ROS1) - Write a ROS Publisher with Python
Scriviamo un publisher in Python
$ cd ~/catkin_ws/src/my_robot_controller/scripts$
lavoreremo sul turtlesim
$ touch draw_circle.py
$ chmod +x draw_circle.py

Vogliamo controllare la velocita' della tartaruga, ci chiediamo quale' il topic:
$rostopic list
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

e' il topic: /turtle1/cmd_vel
invio' un comando di velocita' al nodo, che comanda il disegno della tartaruga:
$ rosrun turtlesim turtlesim_node
mi chiedo che tipo e':
$ rostopic info /turtle1/cmd_vel
Type: geometry_msgs/Twist
...
$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
 float64 x
 float64 y
 float64 z 
geometry_msgs/Vector3 angular
 float64 x
 float64 y
 float64 z 

### draw_circle.py
#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist

if __name__=='__main__':
	rospy.init_node("draw_circle")
	rospy.loginfo("Node has been started.")         dim buffer messaggi
	rate=rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
	rate = rospy.Rate(2)
	
	while not rospy.is_shutdown():
		# publish cmd_vel
		#devo spedire ol messaggio fi tipo :geometry_msgs/Twist
		msg = Twist()
		msg.linear.x = 2.0 #(andra a destra se pos a sx se negatrivo)
		msg.angular.z = 1. #per farla ruotare
		#pubblichiamo il messaggio
		pub.publish(msg)
				
		rate.sleep()
		
Eseguiamo:
$ rosrun my_robot_controller draw_circle.py

$ rostopic info /turtle1/cmd_vel
Type: geometry_msgs/Twist
Publishers:
.....
Subscribers:
.....


Bisogna inserire in package.xml la dipendenza:???
...
<build_depend>geometry_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
...

################ - Write a ROS Subscriber with Python
Creiamo un nodo ROS di tipo Subscriber in Python, cioe' un nodo che vuole ascoltare:
$ rosrun turtlesim turtlesim_node

vediamo quali sono i topic cioe' gli argomenti:
$ rostopic list
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

$ rostopic info /turtle1/pose
abbiamo un solo un publisher e nessun subscriber
$ rostopic echo /turtle1/pose
scriviamo un subscriber:
$ ~/catkin_ws/src/my_robot_controller/scripts$ touch pose_subscriber.py
$ chmod +x pose_subscriber.py

.code
### pose_subscriber.py
#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose

def pose_callbak(msg: Pose):  #funzione chiamata quando arriva un messaggio
	#rospy.loginfo(msg)
	rospy.loginfo("("+ str(msg.x) + ", " + str(msg.y) +")")
	
if __name__=='__main__':
	rospy.init_node("turtle_pose_subscriber")
	rospy.loginfo("Node has been started.")         
	sub=rospy.Subscriber("/turtle1/pose", Pose, calback=pose_callback)
	rospy.loginfo("Node has been started.")        
    
	rospy.spin()#ferma il programma e i callback vengono eseguiti in thread diversi
	
Eseguiamo	
$ rosrun turtlesim turtlesim_node		
$ rosrun my_robot_controller
$ ls
draw_circle.py    my_first_node.py    pose_subscriber.py
$ rosrun my_robot_controller pose_subscriber.py

Labciamo la tastiera:
$ rosrun turtlesim turtle_teleop_key

##################9 - Combine Publisher and Subscriber in a Closed Loop System
Creiamo un nodo che e' sia pub che sub
$ roscore

$rosrun turtlesim turtlesim_mode

andiamo in:
$~/catkin_ws/src/my_robot_controller/scripts$ touch turtle_controller.py

chmod +x turtle_controller.py

### turtle_controller.py
#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist

def pose_callbak(msg: Pose):
	cmd = Ywist()
	cmd.linear.x = 5.0
	cmd.angular.z = 0.0
	pub.publisher(cmd)
	
if __name__=='__main__':
	rospy.init_node("turtle_controller")
	sub=rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
	sub=rospy.Subscriber("/turtle1/pose", Pose, calback=pose_callback)
	rospy.loginfo("Node has been started.")        
      
	rospy.spin()
	
	
$ rosrun my_robot_controller turtle_controller.py
$ rosrun turtlesim turtle_teleop_key

La finestra della tartaruga ha lo 0 0 in basso a sx e 11x11
modifichiamo per restare in un perimetro di sicurezza
### turtle_controller.py
#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist

def pose_callbak(msg: Pose):
	cmd = Ywist()
	if pose.x > 9.0 or pose.x< 2.0 or pose.y > 9.0 or pose.y < 2.0: #se siamo nella zona di pericolo svoltiamo
		cmd.linear.x = 1.0
		cmd.angular.z=1.4
	else:		
		cmd.linear.x = 5.0
		cmd.angular.z = 0.0
	pub.publisher(cmd)
	
if __name__=='__main__':
	rospy.init_node("turtle_controller")
	sub=rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
	sub=rospy.Subscriber("/turtle1/pose", Pose, calback=pose_callback)
	rospy.loginfo("Node has been started.")        
      
	rospy.spin()

riavviamo:
$ rosrun turtlesim turtlesim_node
$ rosrun my_robot_controller turtle_controller.py 

#################10 - Understand What is a ROS Service
Parliamo ora di Servizi ROS
Il meccanismo qui e' client/Server

$ roscore
$ rosrun rospy_tutorials add_two_ints_server

Chiediamo una lista di servizi attivi
$ rosservice list
/add_two_ints
/add_two_ints_server/get_loggers
/add_two_ints_server/set_loggers_level
/rosout/get_loggers
/rosout/set_logger_level

chiedo dettagli sul servizio add_two_ints:
$ rosservice info /add_two_ints
Node: /add_two_ints_server  (nodo che offre il servizio)
Type: rospy_tutorials/AddTwoInts (Tipo del servizio)
Args: a b

Lanciamo il Server:
$ rosrun rospy_tutorials add_two_ints_server
Lancio il Client chiedendo la somma di 2 e 5:
$ rosservice call /add_two_ints "a: 2 b: 5" (??? MY:$ rosservice call /add_two_ints 2 5)
sum: 7

help comandi 
$ rosm
rosmake	rosmsg	rosmv
rosmaster	rosmsg_proto

$ rossrv show rospy_tutorials/AddTwoInts
int64 a
int64 b
...
int64 sum

Lanciamo ora la tartaruga:
$ rosrun turtlesim turtlesim_node

e vediamone i servizi:
$ rosservice list
/clear
/turtlesim/set_pen
...
vediamone uno:
$ service info /turtle1/set_pen
Node: /turtlesim
Type: turtlesim/SetPen
Args: r g b width off

quindi:
$ rossrv show turtlesim/SetPen
uint8 r
uint8 g
uint8 b
uint8 width
uint8 off
...


la risposta e' vuota

$ rosrun turtlesim turtle_teleop_key

$ rosservice call /turtle1/set_pen <TAB> .....

nel rqt_graph non vedo i servizi

################11 - Write a ROS Service Client with Python
Scriviamo un programma in Python che sia un client di un sevizio
#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from turtlesim.srv import SetPen

def call_set_pen_service(r, g, b, width, off):
	try:
		set_pen = rospy.ServiceProxy("/turtle1/set_pen",SetPen)
		response = set_pen(r, g, b, width, off)
		rospy.loginfo(response)
	except tospy.ServiceException as e:
		rospy.logwarn(e)
		
		
def pose_callbak(msg: Pose):
	cmd = Ywist()
	if pose.x > 9.0 or pose.x< 2.0 or pose.y > 9.0 or pose.y < 2.0: #se siamo nella zona di pericolo svoltiamo
		cmd.linear.x = 1.0
		cmd.angular.z=1.4
	else:		
		cmd.linear.x = 5.0
		cmd.angular.z = 0.0
	pub.publisher(cmd)
	
	if pose.x >= 5.5:
		call_set_pen_service(255, 0, 0, 3, 0)
	else:
		call_set_pen_service(0, 255, 0, 3, 0)
if __name__=='__main__':
	rospy.init_node("turtle_controller")
	rospy.wait_for_service("/turtle1/set_pen")#aspetto che il servizio sia attivo
	call_set_pen_service(255, 0, 0, 0, 3, 0)
	sub=rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
	sub=rospy.Subscriber("/turtle1/pose", Pose, calback=pose_callback)
	rospy.loginfo("Node has been started.")        
      
	rospy.spin()

	

$ rosrun my_robot_controller turtle_controller.py
dato che x va da 0 a 11
quindi la soglia e' a 5,5

$ rostopic hz /turtle1/pose
average rate: 61.239
	min...
....

in media 58 cioe' 58 messaggi al secondo quindi pose_callbak() verra' chiamata 58 volte al secondo
sarebbe meglio chiamarla quando ne ho bisogno cioe' quando la tartaruga pass dalla mezzeria di x.
definisco una var previous_x

#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from turtlesim.srv import SetPen

previous_x = 0

def call_set_pen_service(r, g, b, width, off):
	try:
		set_pen = rospy.ServiceProxy("/turtle1/set_pen",SetPen)
		response = set_pen(r, g, b, width, off)
		rospy.loginfo(response)
	except tospy.ServiceException as e:
		rospy.logwarn(e)
		
		
def pose_callbak(msg: Pose):
	cmd = Ywist()
	if pose.x > 9.0 or pose.x< 2.0 or pose.y > 9.0 or pose.y < 2.0: #se siamo nella zona di pericolo svoltiam
		cmd.linear.x = 1.0
		cmd.angular.z=1.4
	else:		
		cmd.linear.x = 5.0
		cmd.angular.z = 0.0
	pub.publisher(cmd)
	
	global previous_x
	if pose.x >= 5.5 and previous_x < 5.5:
		#previous_x = pose.x
		rospy.loginfo("set color to red")
		call_set_pen_service(255, 0, 0, 3, 0)
	elif pose.x < 5.5 and previous_x > = 5.5:
		#previous_x = pose.x
		rospy.loginfo("set color to green")
		call_set_pen_service(0, 255, 0, 3, 0)
	previous_x = pose.x
		
if __name__=='__main__':
	rospy.init_node("turtle_controller")
	rospy.wait_for_service("/turtle1/set_pen")#aspetto che il servizio sia attivo
	call_set_pen_service(255, 0, 0, 0, 3, 0)
	sub=rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
	sub=rospy.Subscriber("/turtle1/pose", Pose, calback=pose_callback)
	rospy.loginfo("Node has been started.")        
      
	rospy.spin()
	
$ rosrun turtlesim turtlesim_node

$ rosrun my_robot_controller turtle_controller.py

======================================================================================