-
-
Save JaldertVicarious/84eaf1c8006e80a4afa30858b296f681 to your computer and use it in GitHub Desktop.
ignition-gazebo-revolute-conveyor-example
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#0 __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51 | |
#1 0x00007ffff7573801 in __GI_abort () at abort.c:79 | |
#2 0x00007ffff75bc897 in __libc_message (action=action@entry=do_abort, fmt=fmt@entry=0x7ffff76e9b9a "%s\ | |
") at ../sysdeps/posix/libc_fatal.c:181 | |
#3 0x00007ffff75c390a in malloc_printerr (str=str@entry=0x7ffff76eb800 "free(): invalid next size (fast)") at malloc.c:5350 | |
#4 0x00007ffff75caf60 in _int_free (have_lock=0, p=0x5555582dfd50, av=0x7ffff791ec40 <main_arena>) at malloc.c:4213 | |
#5 __GI___libc_free (mem=0x5555582dfd60) at malloc.c:3124 | |
#6 0x00007fffbb684f58 in dart::constraint::BoxedLcpConstraintSolver::solveConstrainedGroup(dart::constraint::ConstrainedGroup&) () from /usr/lib/x86_64-linux-gnu/libdart.so.6.10 | |
#7 0x00007fffbb689b19 in dart::constraint::ConstraintSolver::solveConstrainedGroups() () from /usr/lib/x86_64-linux-gnu/libdart.so.6.10 | |
#8 0x00007fffbb68ca9e in dart::constraint::ConstraintSolver::solve() () from /usr/lib/x86_64-linux-gnu/libdart.so.6.10 | |
#9 0x00007fffbb6ae3dc in dart::simulation::World::step(bool) () from /usr/lib/x86_64-linux-gnu/libdart.so.6.10 | |
#10 0x00007fffbbd2ef70 in ignition::physics::dartsim::SimulationFeatures::WorldForwardStep(ignition::physics::Identity const&, ignition::physics::SpecifyData<ignition::physics::RequireData<ignition::physics::WorldPoses>, ignition::physics::ExpectData<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&) () at /home/jaldert/ignition_workspace/src/ign-physics/dartsim/src/SimulationFeatures.cc:53 | |
#11 0x00007fffd44adaf1 in ignition::physics::ForwardStep::World<ignition::physics::FeaturePolicy<double, 3ul>, ignition::gazebo::v3::systems::PhysicsPrivate::MinimumFeatureList>::Step (_u=..., _x=..., _h=..., this=<optimized out>) at /usr/include/c++/8/ext/new_allocator.h:86 | |
#12 ignition::gazebo::v3::systems::PhysicsPrivate::Step(std::chrono::duration<long, std::ratio<1l, 1000000000l> > const&) () at /home/jaldert/ignition_workspace/src/ign-gazebo/src/systems/physics/Physics.cc:822 | |
#13 0x00007fffd44b4ba5 in ignition::gazebo::v3::systems::Physics::Update(ignition::gazebo::v3::UpdateInfo const&, ignition::gazebo::v3::EntityComponentManager&) () at /usr/include/c++/8/bits/unique_ptr.h:342 | |
#14 0x00007ffff4af91de in ignition::gazebo::v3::SimulationRunner::UpdateSystems() () at /home/jaldert/ignition_workspace/src/ign-gazebo/src/SimulationRunner.cc:450 | |
#15 0x00007ffff4afe4c3 in ignition::gazebo::v3::SimulationRunner::Step(ignition::gazebo::v3::UpdateInfo const&) () at /home/jaldert/ignition_workspace/src/ign-gazebo/src/SimulationRunner.cc:687 | |
#16 0x00007ffff4afec6b in ignition::gazebo::v3::SimulationRunner::Run(unsigned long) () at /home/jaldert/ignition_workspace/src/ign-gazebo/src/SimulationRunner.cc:660 | |
#17 0x00007ffff4af5a35 in ignition::gazebo::v3::ServerPrivate::Run(unsigned long, std::optional<std::condition_variable*>) () at /usr/include/c++/8/bits/unique_ptr.h:342 | |
#18 0x00007ffff4af0fa9 in ignition::gazebo::v3::Server::Run(bool, unsigned long, bool) () at /usr/include/c++/8/optional:969 | |
#19 0x00007ffff517b578 in runServer () at /home/jaldert/ignition_workspace/src/ign-gazebo/src/cmd/ign.cc:148 | |
#20 0x00007ffff5592dae in ffi_call_unix64 () from /usr/lib/x86_64-linux-gnu/libffi.so.6 | |
#21 0x00007ffff559271f in ffi_call () from /usr/lib/x86_64-linux-gnu/libffi.so.6 | |
#22 0x00007ffff57998f8 in ?? () from /usr/lib/x86_64-linux-gnu/ruby/2.5.0/fiddle.so | |
#23 0x00007ffff7aa3cab in ?? () from /usr/lib/x86_64-linux-gnu/libruby-2.5.so.2.5 | |
#24 0x00007ffff579968d in ?? () from /usr/lib/x86_64-linux-gnu/ruby/2.5.0/fiddle.so | |
#25 0x00007ffff7aca3c9 in ?? () from /usr/lib/x86_64-linux-gnu/libruby-2.5.so.2.5 | |
#26 0x00007ffff7ad88f3 in ?? () from /usr/lib/x86_64-linux-gnu/libruby-2.5.so.2.5 | |
#27 0x00007ffff7aced85 in ?? () from /usr/lib/x86_64-linux-gnu/libruby-2.5.so.2.5 | |
#28 0x00007ffff7ad4b64 in ?? () from /usr/lib/x86_64-linux-gnu/libruby-2.5.so.2.5 | |
#29 0x00007ffff79af0c4 in ?? () from /usr/lib/x86_64-linux-gnu/libruby-2.5.so.2.5 | |
#30 0x00007ffff79b0f4d in ruby_exec_node () from /usr/lib/x86_64-linux-gnu/libruby-2.5.so.2.5 | |
#31 0x00007ffff79b342e in ruby_run_node () from /usr/lib/x86_64-linux-gnu/libruby-2.5.so.2.5 | |
#32 0x00005555555548cb in ?? () | |
#33 0x00007ffff7554b97 in __libc_start_main (main=0x555555554880, argc=6, argv=0x7fffffffb028, init=<optimized out>, fini=<optimized out>, rtld_fini=<optimized out>, stack_end=0x7fffffffb018) at ../csu/libc-start.c:310 | |
#34 0x00005555555548fa in _start () |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) | |
#============================================================================ | |
# Initialize the project | |
#============================================================================ | |
project(RevoluteConveyorControllerSystem) | |
#============================================================================ | |
# Find ignition-cmake | |
#============================================================================ | |
# If you get an error at this line, you need to install ignition-cmake | |
find_package(ignition-cmake2 REQUIRED) | |
##============================================================================ | |
## Search for project-specific dependencies | |
##============================================================================ | |
# Find ignition-plugin | |
ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) | |
set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) | |
# Find ignition-gazebo | |
ign_find_package(ignition-gazebo3) | |
set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR}) | |
# Set plugin install directory | |
# The destination must be known by ignition::gazebo::SystemLoader for dynamic | |
# loading to work (this is one of the default search locations) | |
set(CUSTOM_IGNITION_GAZEBO_PLUGIN_INSTALL_DIR | |
$ENV{HOME}/.ignition/gazebo/plugins | |
) | |
# Build library | |
add_library(RevoluteConveyorController SHARED RevoluteConveyorController.cc) | |
set_property(TARGET RevoluteConveyorController PROPERTY CXX_STANDARD 17) | |
target_link_libraries(RevoluteConveyorController | |
ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} | |
ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}) | |
install(TARGETS RevoluteConveyorController DESTINATION ${CUSTOM_IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}) | |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<sdf version='1.7'> | |
<world name='conveyor_with_box'> | |
<physics name='physics_settings' type='dart'> | |
<max_step_size>0.001</max_step_size> | |
<real_time_factor>1</real_time_factor> | |
<real_time_update_rate>1000</real_time_update_rate> | |
</physics> | |
<plugin name='ignition::gazebo::systems::Physics' filename='libignition-gazebo-physics-system.so'/> | |
<plugin name='ignition::gazebo::systems::UserCommands' filename='libignition-gazebo-user-commands-system.so'/> | |
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='libignition-gazebo-scene-broadcaster-system.so'/> | |
<plugin name='ignition::gazebo::systems::Sensors' filename='libignition-gazebo-sensors-system.so'> | |
<render_engine>ogre2</render_engine> | |
</plugin> | |
<scene> | |
<grid>1</grid> | |
<ambient>1 1 1 1</ambient> | |
<background>0.8 0.8 0.8 1</background> | |
<shadows>1</shadows> | |
</scene> | |
<gui fullscreen='0'> | |
<plugin name='3D View' filename='GzScene3D'> | |
<ignition-gui> | |
<title>3D View</title> | |
<property type='bool' key='showTitleBar'>0</property> | |
<property type='string' key='state'>docked</property> | |
</ignition-gui> | |
<engine>ogre2</engine> | |
<scene>scene</scene> | |
<ambient_light>1.0 1.0 1.0</ambient_light> | |
<background_color>0.8 0.8 0.8</background_color> | |
<camera_pose>-6 0 6 0 0.5 0</camera_pose> | |
</plugin> | |
<plugin name='World control' filename='WorldControl'> | |
<ignition-gui> | |
<title>World control</title> | |
<property type='bool' key='showTitleBar'>0</property> | |
<property type='bool' key='resizable'>0</property> | |
<property type='double' key='height'>72</property> | |
<property type='double' key='width'>121</property> | |
<property type='double' key='z'>1</property> | |
<property type='string' key='state'>floating</property> | |
<anchors target='3D View'> | |
<line own='left' target='left'/> | |
<line own='bottom' target='bottom'/> | |
</anchors> | |
</ignition-gui> | |
<play_pause>1</play_pause> | |
<step>1</step> | |
<start_paused>1</start_paused> | |
<service>/world/conveyor_with_box/control</service> | |
<stats_topic>/world/conveyor_with_box/stats</stats_topic> | |
</plugin> | |
<plugin name='World stats' filename='WorldStats'> | |
<ignition-gui> | |
<title>World stats</title> | |
<property type='bool' key='showTitleBar'>0</property> | |
<property type='bool' key='resizable'>0</property> | |
<property type='double' key='height'>110</property> | |
<property type='double' key='width'>290</property> | |
<property type='double' key='z'>1</property> | |
<property type='string' key='state'>floating</property> | |
<anchors target='3D View'> | |
<line own='right' target='right'/> | |
<line own='bottom' target='bottom'/> | |
</anchors> | |
</ignition-gui> | |
<sim_time>1</sim_time> | |
<real_time>1</real_time> | |
<real_time_factor>1</real_time_factor> | |
<iterations>1</iterations> | |
<topic>/world/conveyor_with_box/stats</topic> | |
</plugin> | |
<plugin name='Entity tree' filename='EntityTree'> | |
<ignition-gui> | |
<property type='bool' key='showTitleBar'>0</property> | |
<property type='string' key='state'>docked</property> | |
</ignition-gui> | |
</plugin> | |
<plugin name='Transform Control' filename='TransformControl'/> | |
<plugin name='Image Display' filename='ImageDisplay'> | |
<ignition-gui> | |
<property key='state' type='string'>docked</property> | |
</ignition-gui> | |
</plugin> | |
</gui> | |
<light name='sun' type='directional'> | |
<cast_shadows>1</cast_shadows> | |
<pose>0 0 10 0 -0 0</pose> | |
<diffuse>0.8 0.8 0.8 1</diffuse> | |
<specular>0.8 0.8 0.8 1</specular> | |
<attenuation> | |
<range>1000</range> | |
<constant>0.9</constant> | |
<linear>0.01</linear> | |
<quadratic>0.001</quadratic> | |
</attenuation> | |
<direction>-0.5 0.1 -0.9</direction> | |
</light> | |
<model name='ground_plane'> | |
<static>1</static> | |
<link name='link'> | |
<collision name='collision'> | |
<geometry> | |
<plane> | |
<normal>0 0 1</normal> | |
<size>1 1</size> | |
</plane> | |
</geometry> | |
</collision> | |
<visual name='visual'> | |
<geometry> | |
<plane> | |
<normal>0 0 1</normal> | |
<size>100 100</size> | |
</plane> | |
</geometry> | |
<material> | |
<ambient>0.8 0.8 0.8 1</ambient> | |
<diffuse>0.8 0.8 0.8 1</diffuse> | |
<specular>0.8 0.8 0.8 1</specular> | |
</material> | |
</visual> | |
</link> | |
</model> | |
<model name='conveyor'> | |
<joint name='fixed' type='fixed'> | |
<parent>world</parent> | |
<child>base_link</child> | |
</joint> | |
<frame name='conveyor_top_right' attached_to='base_link'> | |
<pose>0 -0.25 0.25 0 -1.5708 0</pose> | |
</frame> | |
<link name='base_link'> | |
<collision name='base_link_collision'> | |
<geometry> | |
<box> | |
<size>0.5 0.5 0.5</size> | |
</box> | |
</geometry> | |
</collision> | |
<visual name='base_link_visual'> | |
<geometry> | |
<box> | |
<size>0.5 0.5 0.5</size> | |
</box> | |
</geometry> | |
</visual> | |
</link> | |
<link name='roller_0'> | |
<pose relative_to='conveyor_top_right'>0 0.025 0 0 -0 0</pose> | |
<collision name='roller_0_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_0_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_0_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_0</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<link name='roller_1'> | |
<pose relative_to='conveyor_top_right'>0 0.075 0 0 -0 0</pose> | |
<collision name='roller_1_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_1_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_1_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_1</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<link name='roller_2'> | |
<pose relative_to='conveyor_top_right'>0 0.125 0 0 -0 0</pose> | |
<collision name='roller_2_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_2_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_2_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_2</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<link name='roller_3'> | |
<pose relative_to='conveyor_top_right'>0 0.175 0 0 -0 0</pose> | |
<collision name='roller_3_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_3_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_3_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_3</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<link name='roller_4'> | |
<pose relative_to='conveyor_top_right'>0 0.225 0 0 -0 0</pose> | |
<collision name='roller_4_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_4_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_4_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_4</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<link name='roller_5'> | |
<pose relative_to='conveyor_top_right'>0 0.275 0 0 -0 0</pose> | |
<collision name='roller_5_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_5_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_5_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_5</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<link name='roller_6'> | |
<pose relative_to='conveyor_top_right'>0 0.325 0 0 -0 0</pose> | |
<collision name='roller_6_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_6_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_6_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_6</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<link name='roller_7'> | |
<pose relative_to='conveyor_top_right'>0 0.375 0 0 -0 0</pose> | |
<collision name='roller_7_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_7_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_7_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_7</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<link name='roller_8'> | |
<pose relative_to='conveyor_top_right'>0 0.425 0 0 -0 0</pose> | |
<collision name='roller_8_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_8_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_8_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_8</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<link name='roller_9'> | |
<pose relative_to='conveyor_top_right'>0 0.475 0 0 -0 0</pose> | |
<collision name='roller_9_collision'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</collision> | |
<visual name='roller_9_visual'> | |
<geometry> | |
<cylinder> | |
<radius>0.025</radius> | |
<length>0.5</length> | |
</cylinder> | |
</geometry> | |
</visual> | |
</link> | |
<joint name='roller_9_joint' type='revolute'> | |
<parent>base_link</parent> | |
<child>roller_9</child> | |
<axis> | |
<xyz>0 0 1</xyz> | |
<limit> | |
<lower>-1e+16</lower> | |
<upper>1e+16</upper> | |
</limit> | |
</axis> | |
</joint> | |
<plugin name='vc_systems::RevoluteConveyorController' filename='libRevoluteConveyorController.so'/> | |
</model> | |
<model name='box'> | |
<pose>0 0 2 0 -0 0</pose> | |
<link name='body'> | |
<inertial> | |
<mass>0.5</mass> | |
<inertia> | |
<ixx>0.05</ixx> | |
<ixy>0</ixy> | |
<ixz>0</ixz> | |
<iyy>0.05</iyy> | |
<iyz>0</iyz> | |
<izz>0.05</izz> | |
</inertia> | |
</inertial> | |
<visual name='visual'> | |
<geometry> | |
<box> | |
<size>0.25 0.25 0.25</size> | |
</box> | |
</geometry> | |
<material> | |
<ambient>0 1 0 1</ambient> | |
<diffuse>0 1 0 1</diffuse> | |
<specular>0.5 0.5 0.5 1</specular> | |
</material> | |
</visual> | |
<collision name='collision'> | |
<geometry> | |
<box> | |
<size>0.25 0.25 0.25</size> | |
</box> | |
</geometry> | |
</collision> | |
</link> | |
</model> | |
<gravity>0 0 -9.8</gravity> | |
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field> | |
<atmosphere type='adiabatic'/> | |
</world> | |
</sdf> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
* Based on ign-gazebo/src/systems/joint_controller/JointController.cc | |
* | |
* Copyright (C) 2019 Open Source Robotics Foundation | |
* Modifications copyright [2020] Vicarious FPC, Inc. | |
* | |
* Licensed under the Apache License, Version 2.0 (the "License"); | |
* you may not use this file except in compliance with the License. | |
* You may obtain a copy of the License at | |
* | |
* http://www.apache.org/licenses/LICENSE-2.0 | |
* | |
* Unless required by applicable law or agreed to in writing, software | |
* distributed under the License is distributed on an "AS IS" BASIS, | |
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | |
* See the License for the specific language governing permissions and | |
* limitations under the License. | |
* | |
*/ | |
#include <ignition/msgs/double.pb.h> | |
#include <ignition/common/Profiler.hh> | |
#include <ignition/plugin/Register.hh> | |
#include <ignition/transport/Node.hh> | |
#include "ignition/gazebo/Model.hh" | |
#include "ignition/gazebo/components/Joint.hh" | |
#include "ignition/gazebo/components/JointVelocityCmd.hh" | |
#include "ignition/gazebo/components/Name.hh" | |
#include "ignition/gazebo/components/ParentEntity.hh" | |
#include "RevoluteConveyorController.hh" | |
using namespace vc_systems; | |
class vc_systems::RevoluteConveyorControllerPrivate | |
{ | |
/// \brief Callback for velocity command subscription | |
/// \param[in] _msg VelocityCmd message | |
public: | |
void OnCmdVel(const ignition::msgs::Double &_msg); | |
/// \brief Ignition communication node. | |
public: | |
ignition::transport::Node node; | |
/// \brief Joint Entities | |
public: | |
std::vector<ignition::gazebo::Entity> jointEntities; | |
/// \brief The prefix for joints that should be controlled | |
public: | |
std::string rollerPrefix; | |
/// \brief Commanded joint velocity | |
public: | |
double velocityCmd; | |
/// \brief mutex to protect jointVelCmd | |
public: | |
std::mutex jointVelCmdMutex; | |
/// \brief Model interface | |
public: | |
ignition::gazebo::Model model{ignition::gazebo::kNullEntity}; | |
}; | |
////////////////////////////////////////////////// | |
RevoluteConveyorController::RevoluteConveyorController() | |
: dataPtr(std::make_unique<RevoluteConveyorControllerPrivate>()) | |
{ | |
} | |
////////////////////////////////////////////////// | |
void RevoluteConveyorController::Configure( | |
const ignition::gazebo::Entity &_entity, | |
const std::shared_ptr<const sdf::Element> &_sdf, | |
ignition::gazebo::EntityComponentManager &_ecm, | |
ignition::gazebo::EventManager & /*_eventMgr*/) | |
{ | |
this->dataPtr->model = ignition::gazebo::Model(_entity); | |
std::string plugin_name = this->dataPtr->model.Name(_ecm); | |
if (!this->dataPtr->model.Valid(_ecm)) | |
{ | |
ignerr << "RevoluteConveyorController plugin should be attached to " | |
<< "a model entity.\n Failed to initialize." << std::endl; | |
return; | |
} | |
// Prefix used for searching for joints to control | |
_sdf->Get<std::string>("roller_prefix", this->dataPtr->rollerPrefix, | |
"roller_"); | |
// Default/initial velocity to set on joints | |
double default_velocity = 0.; | |
_sdf->Get<double>("default_velocity", default_velocity, 1.0); | |
this->dataPtr->velocityCmd = default_velocity; | |
// Get all joints that contain rollerPrefix | |
auto all_joints = _ecm.EntitiesByComponents( | |
ignition::gazebo::components::ParentEntity(_entity), | |
ignition::gazebo::components::Joint()); | |
for (const auto &joint_entity : all_joints) | |
{ | |
auto name = _ecm.Component<ignition::gazebo::components::Name>(joint_entity) | |
->Data(); | |
size_t found = name.find(this->dataPtr->rollerPrefix); | |
if (found != std::string::npos) | |
{ | |
this->dataPtr->jointEntities.push_back(joint_entity); | |
} | |
} | |
// Subscribe to commands | |
std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; | |
this->dataPtr->node.Subscribe( | |
topic, &RevoluteConveyorControllerPrivate::OnCmdVel, this->dataPtr.get()); | |
ignmsg << "RevoluteConveyorController subscribing to Double messages on [" | |
<< topic << "]" << std::endl; | |
} | |
////////////////////////////////////////////////// | |
void RevoluteConveyorController::PreUpdate( | |
const ignition::gazebo::UpdateInfo &_info, | |
ignition::gazebo::EntityComponentManager &_ecm) | |
{ | |
IGN_PROFILE("RevoluteConveyorController::PreUpdate"); | |
// \TODO(anyone) Support rewind | |
if (_info.dt < std::chrono::steady_clock::duration::zero()) | |
{ | |
ignwarn | |
<< "Detected jump back in time [" | |
<< std::chrono::duration_cast<std::chrono::seconds>(_info.dt).count() | |
<< "s]. System may not work properly." << std::endl; | |
} | |
// Nothing left to do if paused. | |
if (_info.paused) return; | |
// Set velocityCmd for all rollers. | |
std::lock_guard<std::mutex> lock(this->dataPtr->jointVelCmdMutex); | |
for (auto &joint_entity : this->dataPtr->jointEntities) | |
{ | |
auto vel = _ecm.Component<ignition::gazebo::components::JointVelocityCmd>( | |
joint_entity); | |
if (!vel) | |
{ | |
_ecm.CreateComponent(joint_entity, | |
ignition::gazebo::components::JointVelocityCmd( | |
{this->dataPtr->velocityCmd})); | |
} | |
else | |
{ | |
*vel = ignition::gazebo::components::JointVelocityCmd( | |
{this->dataPtr->velocityCmd}); | |
} | |
} | |
} | |
////////////////////////////////////////////////// | |
void RevoluteConveyorControllerPrivate::OnCmdVel( | |
const ignition::msgs::Double &_msg) | |
{ | |
std::lock_guard<std::mutex> lock(this->jointVelCmdMutex); | |
this->velocityCmd = _msg.data(); | |
} | |
IGNITION_ADD_PLUGIN(vc_systems::RevoluteConveyorController, | |
ignition::gazebo::System, | |
vc_systems::RevoluteConveyorController::ISystemConfigure, | |
vc_systems::RevoluteConveyorController::ISystemPreUpdate) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
* Based on ign-gazebo/src/systems/joint_controller/JointController.hh | |
* | |
* Copyright (C) 2019 Open Source Robotics Foundation | |
* Modifications copyright [2020] Vicarious FPC, Inc. | |
* | |
* Licensed under the Apache License, Version 2.0 (the "License"); | |
* you may not use this file except in compliance with the License. | |
* You may obtain a copy of the License at | |
* | |
* http://www.apache.org/licenses/LICENSE-2.0 | |
* | |
* Unless required by applicable law or agreed to in writing, software | |
* distributed under the License is distributed on an "AS IS" BASIS, | |
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | |
* See the License for the specific language governing permissions and | |
* limitations under the License. | |
* | |
*/ | |
#ifndef VC_PLUGINS_SYSTEMS_REVOLUTE_CONVEYOR_CONTROLLER_HH_ | |
#define VC_PLUGINS_SYSTEMS_REVOLUTE_CONVEYOR_CONTROLLER_HH_ | |
#include <ignition/gazebo/System.hh> | |
#include <memory> | |
namespace vc_systems | |
{ | |
// Forward declaration | |
class RevoluteConveyorControllerPrivate; | |
/// \brief Joint controller which can be attached to a model with a reference | |
/// to a single joint. Currently only the first axis of a joint is actuated. | |
class RevoluteConveyorController : public ignition::gazebo::System, | |
public ignition::gazebo::ISystemConfigure, | |
public ignition::gazebo::ISystemPreUpdate | |
{ | |
/// \brief Constructor | |
public: | |
RevoluteConveyorController(); | |
/// \brief Destructor | |
public: | |
~RevoluteConveyorController() override = default; | |
// Documentation inherited | |
public: | |
void Configure(const ignition::gazebo::Entity &_entity, | |
const std::shared_ptr<const sdf::Element> &_sdf, | |
ignition::gazebo::EntityComponentManager &_ecm, | |
ignition::gazebo::EventManager &_eventMgr) override; | |
// Documentation inherited | |
public: | |
void PreUpdate(const ignition::gazebo::UpdateInfo &_info, | |
ignition::gazebo::EntityComponentManager &_ecm) override; | |
/// \brief Private data pointer | |
private: | |
std::unique_ptr<RevoluteConveyorControllerPrivate> dataPtr; | |
}; | |
} // namespace vc_systems | |
#endif |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment