[SOLVED] Monitoring Gazebo contacts from state machine


#1

Hello everyone,
I have an issue in retrieving the list of colliding objects in the state machine: for the sake of the simulated protocol I need to trigger an action based on the robot’s contact with an object in the environment but I can’t access the Gazebo contacts list in the state machine.

If I try to detect the collision I’m interested in using gazebo_msgs.ContactsState with a transfer function everything works perfectly, while doing a similar thing in the state machine file outputs an empty list.

The documentation says that SMACH supports every ROS topic but it doesn’t say anything about Gazebo messages. Is there a way to read those in an .exd file?


#2

Hi Fransesco,

In principle the ROS topics indeed should work in a smach. The message type is irrelevant, this is just the data type that the ROS topic is transmitting. Do you get an error in the console or just an empty list? Can you provide your state machine code to help debug?

Best,
Manos


#3

Hi Manos,
I don’t get any error in the console, I just have an empty list.

Here is the code related to the contacts state:

#CONTACT MESSAGE SUBSCRIBER 
import smach
from smach import CBState 
from gazebo_msgs.msg import ContactState
from hbp_nrp_excontrol.logs import clientLogger

FINISHED = 'FINISHED'
ERROR = 'ERROR'
PREEMPTED = 'PREEMPTED'

cs = ContactState()
sm = StateMachine(outcomes=[FINISHED, ERROR, PREEMPTED])

#defining a contact detection function
def log():
    @smach.cb_interface(outcomes=['success', 'aborted'])
    def cb(userdata):
        msg = cs
        clientLogger.info(msg)
        return 'success'

    return CBState(cb)

[…]

    # Touch state
    StateMachine.add(
     "touch_log",
     log(),
     transitions = {"success": "touch_log",
                    "aborted": "ERROR"}
    )

The state machine loops this state and continues to print in the console (which is what I intended to do for now), but the output is:

"info: ’ ’
collision1_name: ’ ’
collision2_name: ’ ’
wrenches: [ ]
total_wrench:
force:
x: 0.0
y: 0.0
z: 0.0
torque:
x: 0.0
y: 0.0
z: 0.0
contacts_positions: []
contacts_normals: []
depths: [] "

If I put msg = cs outside the cb function definition, the message is still empty but only gets printed once (so the issue should not be in the object ContactState that continues to get updated)


#4

Hi Fransesco,

To my understanding the line cs = ContactState() creates a new ContactState message which is in this case empty. This is the reason that the whole logic just prints out an empty message. To read from the topic you need to establish the communication with ROS through a subscriber. There is an example of communication with ROS in $HBP/Experiments/demo_dvs_icub/icub_behavior_switch.exd .If you need more details don’t hesitate to ask. Hope it helps!

Best,
Manos


#5

Hi Manos,
you were right about the new message being created empty recursively.

I managed to create a custom class for the state I’m interested in:

class detectContact(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes['success','aborted'])
        #initializing self attributes
        self.mutex = threading.Lock()
        self.touching = False
        self.subscriber = rospy.Subscriber('/gazebo/contact_point_data',ContactsState, self.callback)

    def callback(self, data):
        self.mutex.acquire()
        clientLogger.info(data)
        self.mutex.release()

    def execute(self):
        return 'success'

And now by adding the state in my SM:

StateMachine.add(
     "detect_contact",
     detectContact(),
     transitions = {"success": "detect_contact",
                    "aborted": "ERROR"}
)

I’m able to print non-empty values.

I’m only having issues in accessing the specific data in the subscriber message: I only want to monitor the collision names but if I write clientLogger.info(data.collision1_name), it raises an attribute error saying 'ContactsState' object has no attribute 'collision1_name'.

How can I filter the contact names in the console?


#6

Nevermind, after some debugging I found the dictionary structure of the message and I’m able to get the names with: data.states[i].collision1_name , data.states[i].collision2_name.

Thanks for clarifying the need of a ROS subscriber Manos!

Best regards,
Francesco