Skip to content

Commit

Permalink
Clean up package
Browse files Browse the repository at this point in the history
  • Loading branch information
nlamprian committed Aug 20, 2020
1 parent 3ea244f commit 2968752
Show file tree
Hide file tree
Showing 6 changed files with 45 additions and 52 deletions.
39 changes: 23 additions & 16 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,32 +1,39 @@
cmake_minimum_required(VERSION 2.8.6 FATAL_ERROR)
cmake_minimum_required(VERSION 2.8.6)
cmake_policy(SET CMP0048 NEW)
project(roboticsgroup_upatras_gazebo_plugins)

# Set CMP0054
cmake_policy(SET CMP0054 NEW)

# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED COMPONENTS
roscpp
gazebo_ros
control_toolbox
find_package(catkin REQUIRED
COMPONENTS
control_toolbox
gazebo_ros
roscpp
)

# Depend on system install of Gazebo
find_package(gazebo REQUIRED)
# Gazebo cxx flags should have all the required C++ flags
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")

find_package(Boost REQUIRED)

catkin_package(
DEPENDS
roscpp
gazebo_ros
INCLUDE_DIRS
include
LIBRARIES
roboticsgroup_upatras_gazebo_mimic_joint_plugin
roboticsgroup_upatras_gazebo_disable_link_plugin
CATKIN_DEPENDS
control_toolbox
gazebo_ros
roscpp
)

include_directories(
include
${catkin_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)

link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} include)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")

add_library(roboticsgroup_upatras_gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp)
target_link_libraries(roboticsgroup_upatras_gazebo_mimic_joint_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
Expand Down
16 changes: 4 additions & 12 deletions include/roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h
Original file line number Diff line number Diff line change
Expand Up @@ -26,31 +26,23 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
// ROS includes
#include <ros/ros.h>

// Boost includes
#include <boost/bind.hpp>

// Gazebo includes
#include <gazebo/common/Plugin.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>

namespace gazebo {

class DisableLinkPlugin : public ModelPlugin {
public:
public:
DisableLinkPlugin();
~DisableLinkPlugin();
virtual ~DisableLinkPlugin() override;

void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
void UpdateChild();
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) override;

private:
private:
// Parameters
std::string link_name_;

bool kill_sim;

// Pointers to the joints
physics::LinkPtr link_;

Expand Down
16 changes: 6 additions & 10 deletions include/roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,26 +29,22 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
// ros_control
#include <control_toolbox/pid.h>

// Boost includes
#include <boost/bind.hpp>

// Gazebo includes
#include <gazebo/common/Plugin.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>

namespace gazebo {

class MimicJointPlugin : public ModelPlugin {
public:
public:
MimicJointPlugin();
~MimicJointPlugin();
virtual ~MimicJointPlugin() override;

virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) override;

void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
private:
void UpdateChild();

private:
// Parameters
std::string joint_name_, mimic_joint_name_, robot_namespace_;
double multiplier_, offset_, sensitiveness_, max_effort_;
Expand All @@ -67,7 +63,7 @@ namespace gazebo {
physics::WorldPtr world_;

// Pointer to the update event connection
event::ConnectionPtr updateConnection;
event::ConnectionPtr update_connection_;
};

}
Expand Down
8 changes: 4 additions & 4 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -33,17 +33,17 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
<url type="bugtracker">https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins/issues</url>

<buildtool_depend>catkin</buildtool_depend>
<build_depend>control_toolbox</build_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>control_toolbox</build_depend>
<build_export_depend>control_toolbox</build_export_depend>
<build_export_depend>gazebo_ros</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>control_toolbox</build_export_depend>
<exec_depend>control_toolbox</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>control_toolbox</exec_depend>

<export>
<gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />
<gazebo_ros plugin_path="${prefix}/lib" />
</export>
</package>
10 changes: 4 additions & 6 deletions src/disable_link_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,14 +26,11 @@ namespace gazebo {

DisableLinkPlugin::DisableLinkPlugin()
{
kill_sim = false;

link_.reset();
}

DisableLinkPlugin::~DisableLinkPlugin()
{
kill_sim = true;
}

void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Expand All @@ -54,11 +51,12 @@ namespace gazebo {
if (link_) {
link_->SetEnabled(false);
// Output some confirmation
ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: \"" << link_name_);
ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: " << link_name_);
}
else
ROS_ERROR_STREAM("Link" << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
ROS_ERROR_STREAM("Link " << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
}

GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin);
}

} // namespace gazebo
8 changes: 4 additions & 4 deletions src/mimic_joint_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ namespace gazebo {

MimicJointPlugin::~MimicJointPlugin()
{
this->updateConnection.reset();
update_connection_.reset();
}

void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Expand All @@ -59,7 +59,6 @@ namespace gazebo {
}

// Check for robot namespace
robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace")) {
robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
}
Expand Down Expand Up @@ -140,7 +139,7 @@ namespace gazebo {

// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&MimicJointPlugin::UpdateChild, this));

// Output some confirmation
Expand Down Expand Up @@ -191,4 +190,5 @@ namespace gazebo {
}

GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin);
}

} // namespace gazebo

0 comments on commit 2968752

Please sign in to comment.