Gazebo - Méthode cinématique

Il nous faut une méthode pour contrôler le circuit et tous ces modules pour cela nous allons appliquer une vitesse sur l'articulation de la barrière et sur les articulations des feux.

Pour la barrière on utilise une liaison de type revolute avec des limites dans sa rotation, ainsi, elle ne peut tourner que de 90° (position haute et basse). On ajoute aussi un peu de friction pour qu'elle ne tombe pas ou ne rebondisse pas, ce qui est une limite de la simulation.

<joint name="base_to_barrier" type="revolute"> 
        <parent link="base_link"/>
        <child link="barrier"/>
        <origin xyz="-0.025 0.025 0.0" rpy="1.57 0 0"/>
        <axis xyz="0 0 1"/>
        <limit effort="1000" lower="-1.57" upper="0" velocity="1.0"/>
        <dynamics damping="0.0" friction="0.1"/>
</joint>

Pour ce qui est des feux on va avoir 3 disques Rouge Jaune et Vert qui utilisent une liaison de type prismatic avec le même genre de limites que pour la barrière, ainsi, chaque disque de couleur apparaîtra ou disparaîtra selon le contrôle en vitesse appliqué.

<joint name="base_to_light_red" type="prismatic">
        <parent link="base_link"/>
        <child link="light_red"/>
        <origin xyz="-0.0333 0.0 0.0" rpy="0 0 0"/>
        <axis xyz="0 0 1"/>
        <limit lower="0.0" upper="0.002" effort="1000.0" velocity="1000.0"/>
        <dynamics damping="0.0" friction="0.1"/>
</joint>

Ensuite, il faut contrôler chacune de ces liaisons pour cela il existe un topic générer par Gazebo, /gazebo/link_states. On va publier sur ce topic l'état de notre objet, dans ce cas ce sera une vitesse:

pubState = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=1)

Il faut faire une petite manipulation avant de pouvoir publier sur la topic car si on ne publie que la vitesse, la position de l'objet sera réinitialisée alors il faut récupérer l'ancienne position de l'objet, ce sera comme déplacer l'objet par petits pas.

# Get the current state of the barrier
current_state = rospy.wait_for_message('/gazebo/link_states', LinkStates)

# Get the index of the barrier in the list of links
ind = current_state.name.index(self.link_name)

# Create the message
msg = LinkState()
msg.link_name = self.link_name
msg.pose = current_state.pose[ind]

# Change the position of the barrier
msg.twist.angular.x = 6 * (-1)**self.state
msg.twist.angular.y = 6 * (-1)**(self.state+1)

# Update the state of the barrier
self.state = not self.state

# Update the state of the traffic light
self.traffic_light.change_state()

# Publish the message
pubState.publish(msg) 

Le problème avec cette méthode c'est que selon l'orientation de la barrière il faut adapter l'axe de la vitesse angulaire appliquée ( x ou y ).

Codes complets utilisés:

Description du feu de signalisation: traffic_light.urdf

Description de la barrière: barrier.urdf

Monde Gazebo du circuit: arene_2.world

Script de contrôle des modules: world_control.py