Fix build and demos

[cboostjvisser]

Hi,

Here the fixes I needed to apply to successfully build and start the demo's.
This might also fix: #2

There is one problem still left. When running the demo denso_robot_moveit_pickandplace.launch.py or denso_robot_moveit_demo.launch.py; planning seems to work, but the joints do not seem to be driven, and the robot "falls" to the ground and stays there.

[CLAassistant]

All committers have signed the CLA.

[rjoomen]

Friendly ping

[Tiryoh]

@cboostjvisser Sorry for the late responding and thank you for the fix. We've confirmed that this PR switches the simulator to the new Gazebo (formerly Ignition) and make the denso_robot_moveit_demo.launch.py working with vs060 on Gazebo.
I couldn't make work this with cobotta and hsr065a1_n32. Does this reproduce?

I recognize that the following issues should be fixed in the future.

• Interactive markers not responding
• Robot falls on the ground

[cboostjvisser]

@cboostjvisser Sorry for the late responding and thank you for the fix. We've confirmed that this PR switches the simulator to the new Gazebo (formerly Ignition) and make the denso_robot_moveit_demo.launch.py working with vs060 on Gazebo. I couldn't make work this with cobotta and hsr065a1_n32. Does this reproduce?

I recognize that the following issues should be fixed in the future.

• Interactive markers not responding
• Robot falls on the ground

I'll try to get some time to test cobotta and hsr065a1_n32.

Any clue on what's needed to fix the two issues you describe?

[Tiryoh]

I'll try to get some time to test cobotta and hsr065a1_n32.

Thank you.

In order to merge this Pull Request, we believe it is necessary to clarify what has been fix. Even if some problems remain, we believe they can be addressed separately for fix. We are looking into the issues as well.

Interactive markers not responding
Robot falls on the ground

These 2 problems are still investigating.

I'm checking gazebo clock settings for the interactive markers. e.g.) rt-net/crane_x7_ros#192
I'm guessing that the URDF file needs to be corrected to fix the "falling" problem.

[cboostjvisser]

Summary:

This PR fixes two issues:

1. Parameters scale_factor and scale_factor are now double/int instead of string
2. Switched the simulator to the new Gazebo

Open issues:

1. Falling/Sagging of simulated robot

   Details

   The falling(sagging) of robot might have to do with the following:

   Robot still saggs if interfaces are not claimed ros-controls/gazebo_ros2_control#250

   Maybe there is no controller spawned? And thus the joints are not actuated.
   Might also explain why setting an initial value for joint_3 of Cobotta doesn't work? (see point 3 of this list)

2. Interactive marker not responding

3. Cobotta spawns in default position which is out of joint_3's limits.

   Details

   This is causing the planning to fail:

   [moveit_ros.fix_start_state_bounds]: Joint 'joint_3' from the starting state is outside bounds by a significant margin: [-4.87507e-13 ] should be in the range [0.314159 ], [2.44346 ] but the error above the ~start_state_max_bounds_error parameter (currently set to 0.100000)

   If have tried to set an initial_value in denso_robot_joint_limits.xacro for joint_3, but that didn't help:

   <joint name="$(arg namespace)joint_3">
   	<command_interface name="position">
   		<param name="min">0.314159</param>
   		<param name="max">2.443461</param>
   	</command_interface>
   	<command_interface name="velocity">
   		<param name="min">0</param>
   		<param name="max">0.068942250783028</param>
   	</command_interface>
   	<state_interface name="position">
   		<param name="initial_value">1.0</param>
   	</state_interface>
   	<state_interface name="velocity"/>
   	<state_interface name="effort"/>
   </joint>
   

4. Hsr065a1_n32 Reports a crash on bringup (denso_robot_bringup.launch.py):

   Crashlog

   [ign gazebo -r-1] [WARN] [1708352106.230082135] [gz_ros2_control]: On init...
   [ign gazebo -r-1] [INFO] [1708352106.230121435] [resource_manager]: Successful initialization of hardware 'IgnitionSystem'
   [ign gazebo -r-1] [INFO] [1708352106.230154535] [resource_manager]: 'configure' hardware 'IgnitionSystem' 
   [ign gazebo -r-1] [INFO] [1708352106.230158735] [gz_ros2_control]: System Successfully configured!
   [ign gazebo -r-1] [INFO] [1708352106.230167036] [resource_manager]: Successful 'configure' of hardware 'IgnitionSystem'
   [ign gazebo -r-1] [INFO] [1708352106.230173136] [resource_manager]: 'activate' hardware 'IgnitionSystem' 
   [ign gazebo -r-1] [INFO] [1708352106.230177036] [resource_manager]: Successful 'activate' of hardware 'IgnitionSystem'
   [ign gazebo -r-1] [INFO] [1708352106.230183036] [gz_ros2_control]: Loading controller_manager
   [rviz2-5] [ERROR] [1708352106.324960355] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
   [ign gazebo -r-1] 
   [ign gazebo -r-1] ODE INTERNAL ERROR 1: assertion "vertices" failed in buildData() [collision_trimesh_internal.cpp:360]
   [ign gazebo -r-1] Stack trace (most recent call last):
   [ign gazebo -r-1] #31   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7fdc9027f44b, in 
   [ign gazebo -r-1] #30   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93e8a088, in rb_nogvl
   [ign gazebo -r-1] #29   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7fdc9027ed6b, in 
   [ign gazebo -r-1] #28   Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7fdc9022b492, in 
   [ign gazebo -r-1] #27   Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7fdc9022ee2d, in 
   [ign gazebo -r-1] #26   Object "/usr/lib/x86_64-linux-gnu/libignition-gazebo6-ign.so.6.16.0", at 0x7fdc8f74899a, in runServer
   [ign gazebo -r-1] #25   Object "/lib/x86_64-linux-gnu/libignition-gazebo6.so.6", at 0x7fdc8f407a79, in 
   [ign gazebo -r-1] #24   Object "/lib/x86_64-linux-gnu/libignition-gazebo6.so.6", at 0x7fdc8f40dc3a, in ignition::gazebo::v6::SimulationRunner::Run(unsigned long)
   [ign gazebo -r-1] #23   Object "/lib/x86_64-linux-gnu/libignition-gazebo6.so.6", at 0x7fdc8f41773d, in ignition::gazebo::v6::SimulationRunner::Step(ignition::gazebo::v6::UpdateInfo const&)
   [ign gazebo -r-1] #22   Object "/lib/x86_64-linux-gnu/libignition-gazebo6.so.6", at 0x7fdc8f40d2fa, in ignition::gazebo::v6::SimulationRunner::UpdateSystems()
   [ign gazebo -r-1] #21   Object "/usr/lib/x86_64-linux-gnu/ign-gazebo-6/plugins/libignition-gazebo-physics-system.so", at 0x7fdc844eda78, in ignition::gazebo::v6::systems::Physics::Update(ignition::gazebo::v6::UpdateInfo const&, ignition::gazebo::v6::EntityComponentManager&)
   [ign gazebo -r-1] #20   Object "/usr/lib/x86_64-linux-gnu/ign-gazebo-6/plugins/libignition-gazebo-physics-system.so", at 0x7fdc844e341e, in ignition::gazebo::v6::systems::PhysicsPrivate::Step(std::chrono::duration<long, std::ratio<1l, 1000000000l> > const&)
   [ign gazebo -r-1] #19   Object "/usr/lib/x86_64-linux-gnu/ign-physics-5/engine-plugins/libignition-physics-dartsim-plugin.so", at 0x7fdc741db5a8, in ignition::physics::dartsim::SimulationFeatures::WorldForwardStep(ignition::physics::Identity const&, ignition::physics::SpecifyData<ignition::physics::RequireData<ignition::physics::WorldPoses>, ignition::physics::ExpectData<ignition::physics::ChangedWorldPoses, ignition::physics::Contacts, ignition::physics::JointPositions> >&, ignition::physics::CompositeData&, ignition::physics::ExpectData<ignition::physics::ApplyExternalForceTorques, ignition::physics::ApplyGeneralizedForces, ignition::physics::VelocityControlCommands, ignition::physics::ServoControlCommands> const&)
   [ign gazebo -r-1] #18   Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fdc69df5e73, in dart::simulation::World::step(bool)
   [ign gazebo -r-1] #17   Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fdc69ddb705, in dart::constraint::ConstraintSolver::solve()
   [ign gazebo -r-1] #16   Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fdc69dd9fe1, in dart::constraint::ConstraintSolver::updateConstraints()
   [ign gazebo -r-1] #15   Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fdc69dacb7c, in dart::collision::CollisionGroup::collide(dart::collision::CollisionOption const&, dart::collision::CollisionResult*)
   [ign gazebo -r-1] #14   Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fdc69dacb0b, in dart::collision::CollisionGroup::update()
   [ign gazebo -r-1] #13   Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fdc69dac474, in dart::collision::CollisionGroup::updateSkeletonSource(std::pair<dart::dynamics::MetaSkeleton const* const, dart::collision::CollisionGroup::CollisionSource<std::weak_ptr<dart::dynamics::MetaSkeleton const>, dart::dynamics::BodyNode> >&)
   [ign gazebo -r-1] #12   Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fdc69dab428, in dart::collision::CollisionGroup::addShapeFrameImpl(dart::dynamics::ShapeFrame const*, void const*)
   [ign gazebo -r-1] #11   Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fdc69da782b, in dart::collision::CollisionDetector::claimCollisionObject(dart::dynamics::ShapeFrame const*)
   [ign gazebo -r-1] #10   Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fdc69da7925, in dart::collision::CollisionDetector::ManagerForUnsharableCollisionObjects::claimCollisionObject(dart::dynamics::ShapeFrame const*)
   [ign gazebo -r-1] #9    Object "/lib/x86_64-linux-gnu/libdart-collision-ode.so.6.12", at 0x7fdc74010322, in dart::collision::OdeCollisionDetector::createCollisionObject(dart::dynamics::ShapeFrame const*)
   [ign gazebo -r-1] #8    Object "/lib/x86_64-linux-gnu/libdart-collision-ode.so.6.12", at 0x7fdc7400f6bd, in dart::collision::OdeCollisionObject::OdeCollisionObject(dart::collision::OdeCollisionDetector*, dart::dynamics::ShapeFrame const*)
   [ign gazebo -r-1] #7    Object "/lib/x86_64-linux-gnu/libdart-collision-ode.so.6.12", at 0x7fdc7400eea5, in dart::collision::detail::OdeMesh::OdeMesh(dart::collision::OdeCollisionObject const*, aiScene const*, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)
   [ign gazebo -r-1] #6    Object "/lib/x86_64-linux-gnu/libode.so.8", at 0x7fdc691cd590, in dGeomTriMeshDataBuildDouble1
   [ign gazebo -r-1] #5    Object "/lib/x86_64-linux-gnu/libode.so.8", at 0x7fdc691cd33e, in dxTriMeshData::buildData(IceMaths::Point const*, int, unsigned int, IceMaths::IndexedTriangle const*, unsigned int, int, double const*, bool)
   [ign gazebo -r-1] #4    Object "/lib/x86_64-linux-gnu/libode.so.8", at 0x7fdc691ce4b4, in dxTriDataBase::buildData(void const*, int, unsigned int, void const*, unsigned int, int, void const*, bool)
   [ign gazebo -r-1] #3    Object "/lib/x86_64-linux-gnu/libode.so.8", at 0x7fdc6919a7b7, in dDebug
   [ign gazebo -r-1] #2    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fdc93a8a7f2, in abort
   [ign gazebo -r-1] #1    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fdc93aa4475, in raise
   [ign gazebo -r-1] #0    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fdc93af89fc, in pthread_kill
   [ign gazebo -r-1] Aborted (Signal sent by tkill() 22852 0)
   [rviz2-5] [INFO] [1708352106.352681795] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
   [ign gazebo -r-1] Stack trace (most recent call last):
   [ign gazebo -r-1] #30   Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
   [ign gazebo -r-1] #29   Object "ign gazebo gui", at 0x5650089241c4, in _start
   [ign gazebo -r-1] #28   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fdc93a8be3f, in __libc_start_main
   [ign gazebo -r-1] #27   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fdc93a8bd8f, in 
   [ign gazebo -r-1] #26   Object "ign gazebo gui", at 0x56500892417e, in 
   [ign gazebo -r-1] #25   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93d35e19, in ruby_run_node
   [ign gazebo -r-1] #24   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93d32317, in 
   [ign gazebo -r-1] #23   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93ec730c, in rb_vm_exec
   [ign gazebo -r-1] #22   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93ec1c96, in 
   [ign gazebo -r-1] #21   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93ebefc5, in 
   [ign gazebo -r-1] #20   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93ebcc34, in 
   [ign gazebo -r-1] #19   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93e08a1e, in 
   [ign gazebo -r-1] #18   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93d339ac, in rb_protect
   [ign gazebo -r-1] #17   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93ecbc61, in rb_yield
   [ign gazebo -r-1] #16   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93ec730c, in rb_vm_exec
   [ign gazebo -r-1] #15   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93ec1c96, in 
   [ign gazebo -r-1] #14   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93ebefc5, in 
   [ign gazebo -r-1] #13   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93ebcc34, in 
   [ign gazebo -r-1] #12   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7fdc9027f44b, in 
   [ign gazebo -r-1] #11   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fdc93e8a088, in rb_nogvl
   [ign gazebo -r-1] #10   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7fdc9027ed6b, in 
   [ign gazebo -r-1] #9    Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7fdc9022b492, in 
   [ign gazebo -r-1] #8    Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7fdc9022ee2d, in 
   [ign gazebo -r-1] #7    Object "/usr/lib/x86_64-linux-gnu/libignition-gazebo6-ign.so.6.16.0", at 0x7fdc8f74586c, in runGui
   [ign gazebo -r-1] #6    Object "/lib/x86_64-linux-gnu/libignition-gazebo6-gui.so.6", at 0x7fdc8f56275b, in ignition::gazebo::v6::gui::runGui(int&, char**, char const*, char const*, int, char const*)
   [ign gazebo -r-1] #5    Object "/lib/x86_64-linux-gnu/libignition-gui6.so.6", at 0x7fdc8e29c18c, in ignition::gui::Application::~Application()
   [ign gazebo -r-1] #4    Object "/lib/x86_64-linux-gnu/libQt5Core.so.5", at 0x7fdc8e5f1923, in QObject::~QObject()
   [ign gazebo -r-1] #3    Object "/lib/x86_64-linux-gnu/libQt5Core.so.5", at 0x7fdc8e5e6a6d, in QObjectPrivate::deleteChildren()
   [ign gazebo -r-1] #2    Object "/lib/x86_64-linux-gnu/libignition-gazebo6-gui.so.6", at 0x7fdc8f56e8ec, in ignition::gazebo::v6::GuiRunner::~GuiRunner()
   [ign gazebo -r-1] #1    Object "/lib/x86_64-linux-gnu/libignition-gazebo6-gui.so.6", at 0x7fdc8f56e8d1, in ignition::gazebo::v6::GuiRunner::~GuiRunner()
   [ign gazebo -r-1] #0    Object "/lib/x86_64-linux-gnu/libignition-gazebo6-gui.so.6", at 0x7fdc8f57637b, in void ignition::utils::detail::DefaultDelete<ignition::gazebo::v6::GuiRunner::Implementation>(ignition::gazebo::v6::GuiRunner::Implementation*)
   [ign gazebo -r-1] Segmentation fault (Address not mapped to object [0x7fdc304eced0])
   [INFO] [ign gazebo -r-1]: process has finished cleanly [pid 22760]
   [rviz2-5] [INFO] [1708352106.640719087] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
   [rviz2-5] [INFO] [1708352106.640993989] [moveit_robot_model.robot_model]: Loading robot model 'hsr065a1_n32'...
   
   

[furukawahiromi]

Hi @cboostjvisser @rjoomen @Tiryoh,

Thank you for your contributions and effort on this pull request.
However, we have decided not to merge it at this time as we have opted to postpone support for Ignition.

We are planning to release a version with support for Gazebo Classic later this month.
Afterward, we plan to carefully consider support for Gazebo Sim.

Sincerely,
[email protected]