search

robotology / yarp

YARP - Yet Another Robot Platform
http://www.yarp.it
Other
525 stars 195 forks source link

PolyDriver gets stuck on close() #1303

Open lothas opened 7 years ago

lothas commented 7 years ago

Every now and then PolyDriver will get stuck when close() is called. I think it happened twice already with the torso device (I'm connecting and disconnecting from Gazebo). Here's the bt from gdb:

#0  0x00007ffff5d3e51d in read () at ../sysdeps/unix/syscall-template.S:84
#1  0x00007ffff39b5c87 in ACE::recv_n_i(int, void*, unsigned long, unsigned long*) () from /usr/lib/libACE-6.3.3.so
#2  0x00007ffff4bf7641 in yarp::os::impl::SocketTwoWayStream::read(yarp::os::Bytes const&) ()
   from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#3  0x00007ffff4bf5a1a in yarp::os::InputStream::readLine(int, bool*) () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#4  0x00007ffff4be61b6 in yarp::os::impl::StreamConnectionReader::expectText(int) () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#5  0x00007ffff4bb82fe in yarp::os::impl::BottleImpl::read(yarp::os::ConnectionReader&) () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#6  0x00007ffff4c8ed02 in yarp::os::impl::Protocol::write(yarp::os::SizedWriter&) () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#7  0x00007ffff4c27ea7 in yarp::os::NetworkBase::write(yarp::os::Contact const&, yarp::os::PortWriter&, yarp::os::PortReader&, yarp::os::ContactStyle const&)
    () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#8  0x00007ffff4c29ce4 in enactConnection(yarp::os::Contact const&, yarp::os::Contact const&, yarp::os::ContactStyle const&, int, bool) [clone .constprop.54]
    () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#9  0x00007ffff4c2aeb3 in metaConnect(yarp::os::ConstString const&, yarp::os::ConstString const&, yarp::os::ContactStyle, int) [clone .constprop.53] ()
   from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#10 0x00007ffff4c2c030 in yarp::os::NetworkBase::disconnect(yarp::os::ConstString const&, yarp::os::ConstString const&, yarp::os::ContactStyle const&) ()
   from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#11 0x00007ffff4c2c0d1 in yarp::os::NetworkBase::disconnect(yarp::os::ConstString const&, yarp::os::ConstString const&, bool) ()
   from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#12 0x00007ffff4bc4243 in yarp::os::impl::Companion::disconnect(char const*, char const*, bool) ()
   from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#13 0x00007ffff4c53367 in yarp::os::impl::PortCore::closeMain() () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#14 0x00007ffff4c542e9 in yarp::os::impl::PortCore::close() () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#15 0x00007ffff4c72983 in yarp::os::Port::close() [clone .localalias.38] () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_OS.so.1
#16 0x00007ffff7894523 in yarp::dev::RemoteControlBoard::close() () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_dev.so.1
#17 0x00007ffff778770f in yarp::dev::PolyDriver::closeMain() () from /home/jspitz/icub/software/src/yarp/build/lib/libYARP_dev.so.1
#18 0x000000000055154e in yarp::dev::PolyDriver::close (this=0x7fffffff6140)
    at /home/jspitz/icub/software/src/yarp/src/libYARP_dev/include/yarp/dev/PolyDriver.h:110

It seems to me like there should be some timeout or other way to perform 2-3 attempts at disconnecting.

barbalberto commented 7 years ago

Hi @lothas,

if I've understood correctly your application is using a remote_controlboard and connects to the robot/gazebo and when you close the application sometimes it get stuck, right?

From the gdb backtrace, it seems that a port is trying to close, but while doing so a pending write operation is performed and this operation requires a reply from the server side. So on a first guess I'd check if there is a a thread still accessingg the port while the application closing procedure starts. Anyway I added an interrupt operation in the close of the remote_controloard which shall unlock the port from any pending operation and leave it free to close.

Can you try it out please?

I created a test branch called remoteControlBoard_closing, so you just need to:

cd $YARP_DIR
git checkout remoteControlBoard_closing
make

Then recompile your application and test it. Please let me know if this fix your problem so I merge it into mainline

barbalberto commented 7 years ago

@lothas any update? Have you tested the changes?

lothas commented 7 years ago

Hi, I was on vacation and haven't tested it yet. I'll give it a try today.

lothas commented 7 years ago

I just checked out your branch and it killed poor iCub... I see no errors though. Gazebo launches as usual but now iCub is dead on the floor. It seems like the joint controllers are not working at all. yarp_output.txt

barbalberto commented 7 years ago

The 'iCub on the floor' situation happens when there is no communication between gazebo and the yarp server or some plugin is not loaded. Run gazebo --verbose and make sure it is able to reach the yarpserver and all required plugins are found.

The change done in this commit is very very unlikely to be the cause. Can you please verify the set-up is correct and try again?

In case something does not work, please attach the output of gazebo

lothas commented 7 years ago

I reverted back to master and iCub revived. Here's the output from Gazebo when yarp is compiled from your branch:

Gazebo multi-robot simulator, version 7.7.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

Gazebo multi-robot simulator, version 7.7.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: ****
[Msg] Publicized address: ****
[Err] [InsertModelWidget.cc:302] Missing model.config for model "/home/jspitz/icub/software/src/icub-gazebo/.git"
[Err] [InsertModelWidget.cc:302] Missing model.config for model "/home/jspitz/icub/software/src/icub-gazebo/worlds"
[INFO]created device <gazebo_imu>. See C++ class GazeboYarpIMUDriver for documentation.
[INFO]No ROS group found in config file ... skipping ROS initialization. 
yarp: Port /icubSim/inertial active at tcp://****:10041/
[INFO]Server Inertial : no ROS initialization required 
[INFO]Starting server Inertial thread
[DEBUG]Writing an Inertial measurement.
[INFO]created wrapper <inertial>. See C++ class yarp::dev::ServerInertial for documentation.
[INFO]No ROS group found in config file ... skipping ROS initialization. 
yarp: Port /icubSim/left_leg/analog:o/rpc:i active at tcp://****:10048/
[INFO]AnalogServer : no ROS initialization required 
[INFO]created wrapper <analogServer>. See C++ class yarp::dev::AnalogWrapper for documentation.
[INFO]created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
yarp: Port /icubSim/left_leg/analog:o active at tcp://****:10049/
[INFO]No ROS group found in config file ... skipping ROS initialization. 
yarp: Port /icubSim/left_foot/analog:o/rpc:i active at tcp://****:10050/
[INFO]AnalogServer : no ROS initialization required 
[INFO]created wrapper <analogServer>. See C++ class yarp::dev::AnalogWrapper for documentation.
[INFO]created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
yarp: Port /icubSim/left_foot/analog:o active at tcp://****:10051/
[INFO]No ROS group found in config file ... skipping ROS initialization. 
yarp: Port /icubSim/right_leg/analog:o/rpc:i active at tcp://****:10052/
[INFO]AnalogServer : no ROS initialization required 
[INFO]created wrapper <analogServer>. See C++ class yarp::dev::AnalogWrapper for documentation.
[INFO]created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
yarp: Port /icubSim/right_leg/analog:o active at tcp://****:10053/
[INFO]No ROS group found in config file ... skipping ROS initialization. 
yarp: Port /icubSim/right_foot/analog:o/rpc:i active at tcp://****:10054/
[INFO]AnalogServer : no ROS initialization required 
[INFO]created wrapper <analogServer>. See C++ class yarp::dev::AnalogWrapper for documentation.
[INFO]created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
yarp: Port /icubSim/right_foot/analog:o active at tcp://****:10055/
[INFO]No ROS group found in config file ... skipping ROS initialization. 
yarp: Port /icubSim/left_arm/analog:o/rpc:i active at tcp://****:10044/
[INFO]AnalogServer : no ROS initialization required 
[INFO]created wrapper <analogServer>. See C++ class yarp::dev::AnalogWrapper for documentation.
[INFO]created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
yarp: Port /icubSim/left_arm/analog:o active at tcp://****:10045/
[Wrn] [msgs.cc:1807] Conversion of sensor type[imu] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[INFO]No ROS group found in config file ... skipping ROS initialization. 
yarp: Port /icubSim/right_arm/analog:o/rpc:i active at tcp://****:10046/
[INFO]AnalogServer : no ROS initialization required 
[INFO]created wrapper <analogServer>. See C++ class yarp::dev::AnalogWrapper for documentation.
[INFO]created device <gazebo_forcetorque>. See C++ class GazeboYarpForceTorqueDriver for documentation.
yarp: Port /icubSim/right_arm/analog:o active at tcp://****:10047/
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[Wrn] [msgs.cc:1807] Conversion of sensor type[force_torque] not suppported.
[INFO]/icubSim/torso : no ROS initialization required 
[INFO]/icubSim/torso  initting YARP initialization 
yarp: Port /icubSim/torso/rpc:i active at tcp://****:10112/
yarp: Port /icubSim/torso/command:i active at tcp://****:10113/
yarp: Port /icubSim/torso/state:o active at tcp://****:10114/
yarp: Port /icubSim/torso/stateExt:o active at tcp://****:10115/
[INFO]created wrapper <controlboardwrapper2>. See C++ class yarp::dev::ControlBoardWrapper2 for documentation.
[DEBUG]found:  torso_yaw torso_yaw 
[DEBUG]found:  torso_roll torso_roll 
[DEBUG]found:  torso_pitch torso_pitch 
[DEBUG]done 
[INFO]min_stiffness param found! 
[INFO]max_stiffness param found! 
[INFO]min_damping param found! 
[INFO]max_damping param found! 
[DEBUG]min_stiffness: [   0.000000   0.000000    0.000000  ] 
[DEBUG]max_stiffness: [   1000.000000    1000.000000     1000.000000  ] 
[DEBUG]min_damping: [   0.000000     0.000000    0.000000  ] 
[DEBUG]max_damping: [   100.000000   100.000000  100.000000  ] 
[DEBUG]Initializing Trajectory Generator with current values 
[INFO]created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO]/icubSim/head : no ROS initialization required 
[INFO]/icubSim/head  initting YARP initialization 
yarp: Port /icubSim/head/rpc:i active at tcp://****:10116/
yarp: Port /icubSim/head/command:i active at tcp://****:10117/
yarp: Port /icubSim/head/state:o active at tcp://****:10118/
yarp: Port /icubSim/head/stateExt:o active at tcp://****:10119/
[INFO]created wrapper <controlboardwrapper2>. See C++ class yarp::dev::ControlBoardWrapper2 for documentation.
[DEBUG]found:  neck_pitch neck_pitch 
[DEBUG]found:  neck_roll neck_roll 
[DEBUG]found:  neck_yaw neck_yaw 
[DEBUG]found:  eyes_tilt eyes_tilt 
[DEBUG]found:  left_eye left_eye 
[DEBUG]found:  right_eye right_eye 
[DEBUG]Requested couplings: COUPLING (eyes_vergence_control (4 5) (eyes_version eyes_vergence)) 
[DEBUG]Size:  2 
[INFO]using eyes_vergence_control 
[DEBUG]done 
[INFO]min_stiffness param found! 
[INFO]max_stiffness param found! 
[INFO]min_damping param found! 
[INFO]max_damping param found! 
[DEBUG]min_stiffness: [   0.000000   0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_stiffness: [   1000.000000    1000.000000     1000.000000     1000.000000     1000.000000     1000.000000  ] 
[DEBUG]min_damping: [   0.000000     0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_damping: [   100.000000   100.000000  100.000000  100.000000  100.000000  100.000000  ] 
[DEBUG]Initializing Trajectory Generator with current values 
[INFO]created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO]/icubSim/left_arm : no ROS initialization required 
[INFO]/icubSim/left_arm  initting YARP initialization 
yarp: Port /icubSim/left_arm/rpc:i active at tcp://****:10120/
yarp: Port /icubSim/left_arm/command:i active at tcp://****:10121/
yarp: Port /icubSim/left_arm/state:o active at tcp://****:10122/
yarp: Port /icubSim/left_arm/stateExt:o active at tcp://****:10123/
[INFO]created wrapper <controlboardwrapper2>. See C++ class yarp::dev::ControlBoardWrapper2 for documentation.
[DEBUG]found:  l_shoulder_pitch l_shoulder_pitch 
[DEBUG]found:  l_shoulder_roll l_shoulder_roll 
[DEBUG]found:  l_shoulder_yaw l_shoulder_yaw 
[DEBUG]found:  l_elbow l_elbow 
[DEBUG]found:  l_wrist_prosup l_wrist_prosup 
[DEBUG]found:  l_wrist_pitch l_wrist_pitch 
[DEBUG]found:  l_wrist_yaw l_wrist_yaw 
[DEBUG]done 
[INFO]min_stiffness param found! 
[INFO]max_stiffness param found! 
[INFO]min_damping param found! 
[INFO]max_damping param found! 
[DEBUG]min_stiffness: [   0.000000   0.000000    0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_stiffness: [   1000.000000    1000.000000     1000.000000     1000.000000     1000.000000     1000.000000     1000.000000  ] 
[DEBUG]min_damping: [   0.000000     0.000000    0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_damping: [   100.000000   100.000000  100.000000  100.000000  100.000000  100.000000 100.000000  ] 
[DEBUG]INITIAL CONFIGURATION IS:  -0.520000  0.520000    0.000000    0.785000    0.000000    0.000000    0.698000 
[DEBUG]Initializing Trajectory Generator with default values 
[INFO]created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO]/icubSim/right_arm : no ROS initialization required 
[INFO]/icubSim/right_arm  initting YARP initialization 
yarp: Port /icubSim/right_arm/rpc:i active at tcp://****:10124/
yarp: Port /icubSim/right_arm/command:i active at tcp://****:10125/
yarp: Port /icubSim/right_arm/state:o active at tcp://****:10126/
yarp: Port /icubSim/right_arm/stateExt:o active at tcp://****:10127/
[INFO]created wrapper <controlboardwrapper2>. See C++ class yarp::dev::ControlBoardWrapper2 for documentation.
[DEBUG]found:  r_shoulder_pitch r_shoulder_pitch 
[DEBUG]found:  r_shoulder_roll r_shoulder_roll 
[DEBUG]found:  r_shoulder_yaw r_shoulder_yaw 
[DEBUG]found:  r_elbow r_elbow 
[DEBUG]found:  r_wrist_prosup r_wrist_prosup 
[DEBUG]found:  r_wrist_pitch r_wrist_pitch 
[DEBUG]found:  r_wrist_yaw r_wrist_yaw 
[DEBUG]done 
[INFO]min_stiffness param found! 
[INFO]max_stiffness param found! 
[INFO]min_damping param found! 
[INFO]max_damping param found! 
[DEBUG]min_stiffness: [   0.000000   0.000000    0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_stiffness: [   1000.000000    1000.000000     1000.000000     1000.000000     1000.000000     1000.000000     1000.000000  ] 
[DEBUG]min_damping: [   0.000000     0.000000    0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_damping: [   100.000000   100.000000  100.000000  100.000000  100.000000  100.000000 100.000000  ] 
[DEBUG]INITIAL CONFIGURATION IS:  -0.520000  0.520000    0.000000    0.785000    0.000000    0.000000    0.698000 
[DEBUG]Initializing Trajectory Generator with default values 
[INFO]created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO]/icubSim/left_leg : no ROS initialization required 
[INFO]/icubSim/left_leg  initting YARP initialization 
yarp: Port /icubSim/left_leg/rpc:i active at tcp://****:10056/
yarp: Port /icubSim/left_leg/command:i active at tcp://****:10057/
yarp: Port /icubSim/left_leg/state:o active at tcp://****:10058/
yarp: Port /icubSim/left_leg/stateExt:o active at tcp://****:10059/
[INFO]created wrapper <controlboardwrapper2>. See C++ class yarp::dev::ControlBoardWrapper2 for documentation.
[DEBUG]found:  l_hip_pitch l_hip_pitch 
[DEBUG]found:  l_hip_roll l_hip_roll 
[DEBUG]found:  l_hip_yaw l_hip_yaw 
[DEBUG]found:  l_knee l_knee 
[DEBUG]found:  l_ankle_pitch l_ankle_pitch 
[DEBUG]found:  l_ankle_roll l_ankle_roll 
[DEBUG]done 
[INFO]min_stiffness param found! 
[INFO]max_stiffness param found! 
[INFO]min_damping param found! 
[INFO]max_damping param found! 
[DEBUG]min_stiffness: [   0.000000   0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_stiffness: [   1000.000000    1000.000000     1000.000000     1000.000000     1000.000000     1000.000000  ] 
[DEBUG]min_damping: [   0.000000     0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_damping: [   100.000000   100.000000  100.000000  100.000000  100.000000  100.000000  ] 
[DEBUG]Initializing Trajectory Generator with current values 
[INFO]created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
[INFO]/icubSim/right_leg : no ROS initialization required 
[INFO]/icubSim/right_leg  initting YARP initialization 
yarp: Port /icubSim/right_leg/rpc:i active at tcp://****:10060/
yarp: Port /icubSim/right_leg/command:i active at tcp://****:10061/
yarp: Port /icubSim/right_leg/state:o active at tcp://****:10062/
yarp: Port /icubSim/right_leg/stateExt:o active at tcp://****:10063/
[INFO]created wrapper <controlboardwrapper2>. See C++ class yarp::dev::ControlBoardWrapper2 for documentation.
[DEBUG]found:  r_hip_pitch r_hip_pitch 
[DEBUG]found:  r_hip_roll r_hip_roll 
[DEBUG]found:  r_hip_yaw r_hip_yaw 
[DEBUG]found:  r_knee r_knee 
[DEBUG]found:  r_ankle_pitch r_ankle_pitch 
[DEBUG]found:  r_ankle_roll r_ankle_roll 
[DEBUG]done 
[INFO]min_stiffness param found! 
[INFO]max_stiffness param found! 
[INFO]min_damping param found! 
[INFO]max_damping param found! 
[DEBUG]min_stiffness: [   0.000000   0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_stiffness: [   1000.000000    1000.000000     1000.000000     1000.000000     1000.000000     1000.000000  ] 
[DEBUG]min_damping: [   0.000000     0.000000    0.000000    0.000000    0.000000    0.000000  ] 
[DEBUG]max_damping: [   100.000000   100.000000  100.000000  100.000000  100.000000  100.000000  ] 
[DEBUG]Initializing Trajectory Generator with current values 
[INFO]created device <gazebo_controlboard>. See C++ class GazeboYarpControlBoardDriver for documentation.
yarp: Port /icubSim/applyExternalWrench/rpc:i active at tcp://****:10128/
barbalberto commented 7 years ago

From the Gazebo output everything seems ok. Is the iCub still on the floor? If so it is very strange because the only change is in a device which is not even used by gazebo, that's what makes me think the cause is somewhere else. (Obvioulsy I tested before pushing and it works fine for me) The only thing I can think of is that after the YARP compilation, the gazebo-yarp plugin shall be recompiled and installed to get updated.

Anyway, going back to the initial issue can you please clarify the whole setup?

If I've understood correctly your application is using a remote_controlboard, it connects to Gazebo and when you close your application sometimes it get stuck, right? Or is gazebo getting stuck?

What do you mean by:

I'm connecting and disconnecting from Gazebo.

How do you do it? Is the yarpMotorGui working and closing fine when connected to the iCub in Gazebo? Which iCub model is used in Gazebo?

lothas commented 7 years ago

Regarding the two YARP branches: I switched from master to remoteControlboard_closing, "made", "made installed" and iCub died. I switched back to master, "made", "made installed" and iCub was standing again. The problem might be on my side but it's definitely showing up only on the remoteControlboard_closing branch...

About my setup: I normally open yarpserver, gazebo, rviz and then run my code. For each iteration inside my code, an interface object is created that connects to gazebo through yarp, runs an episode, disconnects and is destroyed. See attached (specifically the connectGazebo and disconnectGazebo methods). icub_yarp_gazebo.txt

barbalberto commented 7 years ago

Hi @lothas, the functions connectGazebo and disconnectGazebo looks good to me. I built a simple example calling those 2 functions in loop for hundreds of times, and it never got stuck. I had to remove some functionality like thread instantiation and boost stuff in order to have it compiling, but the core point of the issue (connecting and disconnecting to Gazebo with the PolyDriver) is there and it works fine.

As for now I'm not able to reproduce this behaviour, can you please provide me a simplified version of your code where this "get stuck" behaviour is shown? It'll help debugging.

Thanks

lothas commented 7 years ago

Well, the threads could likely be the source of the issue. I'll prepare a simpler code and attach it here.

lothas commented 7 years ago

yarp_gazebo_test.tar.gz

barbalberto commented 7 years ago

Thanks, I'll test it asap

barbalberto commented 7 years ago

One question: what is the libGzResetPlugin.so? I think it is a plugin for Gazebo, but it is not clear to me how and when to load it. Can you tell me more about it?

lothas commented 7 years ago

The plug-in was included in the code I attached. You should make and make install. Then make sure gazebo can find it.

On Aug 8, 2017 09:52, "Alberto Cardellino" notifications@github.com wrote:

One question: what is the libGzResetPlugin.so? I think it is a plugin for Gazebo, but it is not clear to me how and when to load it. Can you tell me more about it?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/robotology/yarp/issues/1303#issuecomment-320879798, or mute the thread https://github.com/notifications/unsubscribe-auth/ACktlhFEZ_t2ZcXZOxWFj3gFa1_FkCSsks5sWBOvgaJpZM4OfVOq .

barbalberto commented 7 years ago

Ok, I did it but when I run the test it remains in the loop:

Trying to connect to /icubSim/GzResetPlugin/rpc:i
Trying to connect to /icubSim/GzResetPlugin/rpc:i
Trying to connect to /icubSim/GzResetPlugin/rpc:i
Trying to connect to /icubSim/GzResetPlugin/rpc:i
Trying to connect to /icubSim/GzResetPlugin/rpc:i

As far as I know a ModelPlugin shall be loaded in Gazebo through a urdf/sdf right? I started Gazebo, insert an iCub model and then run the test. No port /icubSim/GzResetPlugin/rpc:i is created and gazebo is not complainig, it look like it is not even trying to load the plugin. Am I missing a step? Is a special robot sdf model required to load it? Or can I skip this plugin for this test?

lothas commented 7 years ago

Oh yes, of course. Our icub model has an added plugin:

<plugin name="GzResetPlugin" filename="libGzResetPlugin.so" >
<robotNamefromConfigFile>model://icub_description/conf/gazebo_icub_robotname.ini</robotNamefromConfigFile>
</plugin>

You can add it under controlboard_right_leg. If you want to skip the plugin I can remove it from the test code.

barbalberto commented 7 years ago

Ok I can run the test and it got stuck. I'll let you when I find the cause.

lothas commented 7 years ago

Hi Alberto, any news on this?

barbalberto commented 7 years ago

Hi @lothas, not yet sorry, I've been very busy. A coworker of mine is setting up everything to help debugging.

barbalberto commented 7 years ago

The reason I found for the remoteControlBoard to be blocked is that sometime a port connection fail and the device is trying to read from a port connected to nothing. The check on connections was referring to a deprecated fallback behaviour already removed, so in the PR #1332 and now the device fails to open and exit, as it should. (I had to update your test case to handle the opening failure of a device. Here is the version I use yg_test.cpp.zip )

It is not happening anymore to me now. Can you test it?

lothas commented 7 years ago

It seems that you also changed IcubYarpGazebo, since in my code connectGazebo() is void but yours is bool.

barbalberto commented 7 years ago

Right! here is both src and include issue1303.zip

lothas commented 7 years ago

So far I haven't been able to run it for a single full time :(

It just now crashed Gazebo with:

yarp: Sending output from /icub_description/left_leg/analog:o to /YarpGazebo/left_leg/analog:i using udp
gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:648: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gazebo::physics::Joint; typename boost::detail::sp_member_access<T>::type = gazebo::physics::Joint*]: Assertion `px != 0' failed.
lothas commented 7 years ago

Gazebo crashed a few times with the same error... This time I got stuck:

Finished running episode
Finished thread : Right Leg
Finished thread : Right Arm
Finished thread : Left Arm
Finished thread : Left Leg
Finished thread : Head
Finished thread : Torso
yarp: Removing output from /YarpGazebo/torso/rpc:o to /icub_description/torso/rpc:i
yarp: Removing output from /YarpGazebo/torso/command:o to /icub_description/torso/command:i
yarp: Removing input from /icub_description/torso/state:o to /YarpGazebo/torso/state:i
yarp: Removing input from /icub_description/torso/stateExt:o to /YarpGazebo/torso/stateExt:i
yarp: Removing output from /YarpGazebo/head/rpc:o to /icub_description/head/rpc:i
yarp: Removing output from /YarpGazebo/head/command:o to /icub_description/head/command:i
yarp: Removing input from /icub_description/head/state:o to /YarpGazebo/head/state:i
yarp: Removing input from /icub_description/head/stateExt:o to /YarpGazebo/head/stateExt:i
yarp: Removing output from /YarpGazebo/right_arm/rpc:o to /icub_description/right_arm/rpc:i
yarp: Removing output from /YarpGazebo/right_arm/command:o to /icub_description/right_arm/command:i
yarp: Removing input from /icub_description/right_arm/state:o to /YarpGazebo/right_arm/state:i
yarp: Removing input from /icub_description/right_arm/stateExt:o to /YarpGazebo/right_arm/stateExt:i
yarp: Removing output from /YarpGazebo/left_arm/rpc:o to /icub_description/left_arm/rpc:i
yarp: Removing output from /YarpGazebo/left_arm/command:o to /icub_description/left_arm/command:i
yarp: Removing input from /icub_description/left_arm/state:o to /YarpGazebo/left_arm/state:i
yarp: Removing input from /icub_description/left_arm/stateExt:o to /YarpGazebo/left_arm/stateExt:i
yarp: Removing output from /YarpGazebo/right_leg/rpc:o to /icub_description/right_leg/rpc:i
yarp: Removing output from /YarpGazebo/right_leg/command:o to /icub_description/right_leg/command:i
yarp: Removing input from /icub_description/right_leg/state:o to /YarpGazebo/right_leg/state:i
yarp: Removing input from /icub_description/right_leg/stateExt:o to /YarpGazebo/right_leg/stateExt:i
yarp: Removing output from /YarpGazebo/left_leg/rpc:o to /icub_description/left_leg/rpc:i
yarp: Removing output from /YarpGazebo/left_leg/command:o to /icub_description/left_leg/command:i
yarp: Removing input from /icub_description/left_leg/state:o to /YarpGazebo/left_leg/state:i
yarp: Removing input from /icub_description/left_leg/stateExt:o to /YarpGazebo/left_leg/stateExt:i
Waiting for Gazebo to disconnect...
yarp: Removing input from /icub_description/left_leg/analog:o to /YarpGazebo/left_leg/analog:i
yarp: Removing input from /icub_description/right_leg/analog:o to /YarpGazebo/right_leg/analog:i
yarp: Removing input from /icub_description/left_arm/analog:o to /YarpGazebo/left_arm/analog:i
yarp: Removing input from /icub_description/right_arm/analog:o to /YarpGazebo/right_arm/analog:i
Requesting a reset to a height of 0.45... "[ACK] Reset request received"
yarp: Port /YarpGazebo/torso/rpc:o active at tcp://152.81.9.181:10042/
yarp: Port /YarpGazebo/torso/command:o active at tcp://152.81.9.181:10043/
yarp: Port /YarpGazebo/torso/state:i active at tcp://152.81.9.181:10044/
yarp: Port /YarpGazebo/torso/stateExt:i active at tcp://152.81.9.181:10045/
yarp: Sending output from /YarpGazebo/torso/rpc:o to /icub_description/torso/rpc:i using tcp
yarp: Sending output from /YarpGazebo/torso/command:o to /icub_description/torso/command:i using udp
yarp: Receiving input from /icub_description/torso/state:o to /YarpGazebo/torso/state:i using udp
yarp: Receiving input from /icub_description/torso/stateExt:o to /YarpGazebo/torso/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
Added Device on port /icub_description/torso
yarp: Port /YarpGazebo/torso/analog:i active at tcp://152.81.9.181:10046/
yarp: Port /YarpGazebo/head/rpc:o active at tcp://152.81.9.181:10047/
yarp: Port /YarpGazebo/head/command:o active at tcp://152.81.9.181:10048/
yarp: Port /YarpGazebo/head/state:i active at tcp://152.81.9.181:10049/
yarp: Port /YarpGazebo/head/stateExt:i active at tcp://152.81.9.181:10050/
yarp: Sending output from /YarpGazebo/head/rpc:o to /icub_description/head/rpc:i using tcp
yarp: Sending output from /YarpGazebo/head/command:o to /icub_description/head/command:i using udp
yarp: Receiving input from /icub_description/head/state:o to /YarpGazebo/head/state:i using udp
yarp: Receiving input from /icub_description/head/stateExt:o to /YarpGazebo/head/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
Added Device on port /icub_description/head
yarp: Port /YarpGazebo/head/analog:i active at tcp://152.81.9.181:10051/
yarp: Port /YarpGazebo/right_arm/rpc:o active at tcp://152.81.9.181:10052/
yarp: Port /YarpGazebo/right_arm/command:o active at tcp://152.81.9.181:10053/
yarp: Port /YarpGazebo/right_arm/state:i active at tcp://152.81.9.181:10054/
yarp: Port /YarpGazebo/right_arm/stateExt:i active at tcp://152.81.9.181:10055/
yarp: Sending output from /YarpGazebo/right_arm/rpc:o to /icub_description/right_arm/rpc:i using tcp
yarp: Sending output from /YarpGazebo/right_arm/command:o to /icub_description/right_arm/command:i using udp
yarp: Receiving input from /icub_description/right_arm/state:o to /YarpGazebo/right_arm/state:i using udp
yarp: Receiving input from /icub_description/right_arm/stateExt:o to /YarpGazebo/right_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
Added Device on port /icub_description/right_arm
yarp: Port /YarpGazebo/right_arm/analog:i active at tcp://152.81.9.181:10056/
yarp: Receiving input from /icub_description/right_arm/analog:o to /YarpGazebo/right_arm/analog:i using udp
yarp: Port /YarpGazebo/left_arm/rpc:o active at tcp://152.81.9.181:10057/
yarp: Port /YarpGazebo/left_arm/command:o active at tcp://152.81.9.181:10058/
yarp: Port /YarpGazebo/left_arm/state:i active at tcp://152.81.9.181:10059/
yarp: Port /YarpGazebo/left_arm/stateExt:i active at tcp://152.81.9.181:10060/
yarp: Sending output from /YarpGazebo/left_arm/rpc:o to /icub_description/left_arm/rpc:i using tcp
yarp: Sending output from /YarpGazebo/left_arm/command:o to /icub_description/left_arm/command:i using udp
yarp: Receiving input from /icub_description/left_arm/state:o to /YarpGazebo/left_arm/state:i using udp
yarp: Receiving input from /icub_description/left_arm/stateExt:o to /YarpGazebo/left_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
Added Device on port /icub_description/left_arm
yarp: Port /YarpGazebo/left_arm/analog:i active at tcp://152.81.9.181:10061/
yarp: Receiving input from /icub_description/left_arm/analog:o to /YarpGazebo/left_arm/analog:i using udp
yarp: Port /YarpGazebo/right_leg/rpc:o active at tcp://152.81.9.181:10062/
yarp: Port /YarpGazebo/right_leg/command:o active at tcp://152.81.9.181:10063/
yarp: Port /YarpGazebo/right_leg/state:i active at tcp://152.81.9.181:10064/
yarp: Port /YarpGazebo/right_leg/stateExt:i active at tcp://152.81.9.181:10065/
yarp: Sending output from /YarpGazebo/right_leg/rpc:o to /icub_description/right_leg/rpc:i using tcp
yarp: Sending output from /YarpGazebo/right_leg/command:o to /icub_description/right_leg/command:i using udp
yarp: Receiving input from /icub_description/right_leg/state:o to /YarpGazebo/right_leg/state:i using udp
yarp: Receiving input from /icub_description/right_leg/stateExt:o to /YarpGazebo/right_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
Added Device on port /icub_description/right_leg
yarp: Port /YarpGazebo/right_leg/analog:i active at tcp://152.81.9.181:10066/
yarp: Receiving input from /icub_description/right_leg/analog:o to /YarpGazebo/right_leg/analog:i using udp
yarp: Port /YarpGazebo/left_leg/rpc:o active at tcp://152.81.9.181:10067/
yarp: Port /YarpGazebo/left_leg/command:o active at tcp://152.81.9.181:10068/
yarp: Port /YarpGazebo/left_leg/state:i active at tcp://152.81.9.181:10069/
yarp: Port /YarpGazebo/left_leg/stateExt:i active at tcp://152.81.9.181:10070/
yarp: Sending output from /YarpGazebo/left_leg/rpc:o to /icub_description/left_leg/rpc:i using tcp
yarp: Sending output from /YarpGazebo/left_leg/command:o to /icub_description/left_leg/command:i using udp
yarp: Receiving input from /icub_description/left_leg/state:o to /YarpGazebo/left_leg/state:i using udp
yarp: Receiving input from /icub_description/left_leg/stateExt:o to /YarpGazebo/left_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
Added Device on port /icub_description/left_leg
yarp: Port /YarpGazebo/left_leg/analog:i active at tcp://152.81.9.181:10071/
yarp: Receiving input from /icub_description/left_leg/analog:o to /YarpGazebo/left_leg/analog:i using udp
waiting for encoders
waiting for torques
POSITION_DIRECT mode set properly
Created thread : Torso
Created thread : Head
Created thread : Left Arm
Created thread : Right Arm
Created thread : Left Leg
iCub YARP-Gazebo connector started
Waiting for Gazebo to connect...
Created thread : Right Leg
Finished running episode
Finished thread : Left Arm
Finished thread : Head
Finished thread : Torso
Finished thread : Left Leg
Finished thread : Right Arm
Finished thread : Right Leg
yarp: Removing output from /YarpGazebo/torso/rpc:o to /icub_description/torso/rpc:i