Skip to content

Instantly share code, notes, and snippets.

@JaldertVicarious

JaldertVicarious/CMakeLists.txt Secret

Last active February 3, 2020 19:29
Show Gist options
  • Save JaldertVicarious/84eaf1c8006e80a4afa30858b296f681 to your computer and use it in GitHub Desktop.
Save JaldertVicarious/84eaf1c8006e80a4afa30858b296f681 to your computer and use it in GitHub Desktop.
Download ZIP
ignition-gazebo-revolute-conveyor-example
Raw
backtrace.txt
#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 ()
Raw
CMakeLists.txt
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})
Raw
conveyor_with_box.sdf
<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>
Raw
RevoluteConveyorController.cc
/*
* 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)
Raw
RevoluteConveyorController.hh
/*
* 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