Skip to content

Commit

Permalink
Enable linters and make them happy
Browse files Browse the repository at this point in the history
  • Loading branch information
chapulina committed Jul 18, 2018
1 parent a7af591 commit dee249e
Show file tree
Hide file tree
Showing 6 changed files with 61 additions and 50 deletions.
6 changes: 6 additions & 0 deletions gazebo_ros/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,11 @@ ament_export_libraries(gazebo_ros_node)
ament_export_dependencies(rclcpp)
ament_export_dependencies(gazebo_dev)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()

install(DIRECTORY include/
Expand All @@ -35,3 +40,4 @@ install(TARGETS gazebo_ros_node
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)

9 changes: 4 additions & 5 deletions gazebo_ros/include/gazebo_ros/executor.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef GAZEBO_ROS_EXECUTOR_HPP
#define GAZEBO_ROS_EXECUTOR_HPP
#ifndef GAZEBO_ROS__EXECUTOR_HPP_
#define GAZEBO_ROS__EXECUTOR_HPP_

#include <rclcpp/rclcpp.hpp>
#include <gazebo/common/common.hh>
Expand Down Expand Up @@ -47,6 +47,5 @@ class Executor : public rclcpp::executors::MultiThreadedExecutor
/// Connection to gazebo sigint event
gazebo::event::ConnectionPtr sigint_handle_;
};

} // namespace gazebo_ros
#endif
} // namespace gazebo_ros
#endif // GAZEBO_ROS__EXECUTOR_HPP_
57 changes: 28 additions & 29 deletions gazebo_ros/include/gazebo_ros/node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,21 +12,21 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef GAZEBO_ROS_NODE_HPP
#define GAZEBO_ROS_NODE_HPP

#include <memory>
#include <mutex>
#include <atomic>
#ifndef GAZEBO_ROS__NODE_HPP_
#define GAZEBO_ROS__NODE_HPP_

#include <rclcpp/rclcpp.hpp>
#include <gazebo/common/common.hh>

#include <gazebo_ros/executor.hpp>

#include <atomic>
#include <memory>
#include <mutex>
#include <string>
#include <utility>

namespace gazebo_ros
{

/// ROS Node for gazebo plugins
/**
* \class Node node.hpp <gazebo_ros/node.hpp>
Expand All @@ -35,14 +35,14 @@ namespace gazebo_ros
*/
class Node : public rclcpp::Node
{
public:
/// Exception thrown when a #Create is called for a Node before #InitROS has ever been called
class NotInitializedException : public std::runtime_error
{
public:
NotInitializedException();
public:
NotInitializedException();
};

public:
/// Shared pointer to a #gazebo_ros::Node
typedef std::shared_ptr<Node> SharedPtr;

Expand All @@ -55,7 +55,7 @@ class Node : public rclcpp::Node
* \param[in] node_name Name for the new node to create
* \return A shared pointer to a new #gazebo_ros::Node
*/
static SharedPtr Create(const std::string& node_name);
static SharedPtr Create(const std::string & node_name);

/// Create a #gazebo_ros::Node and add it to the global #gazebo_ros::Executor.
/**
Expand Down Expand Up @@ -86,7 +86,7 @@ class Node : public rclcpp::Node
* \param[in] _sdf An SDF element which either is a <ros> element or contains a <ros> element
* \return A shared pointer to a new #gazebo_ros::Node
*/
static SharedPtr Create(const std::string& node_name, sdf::ElementPtr _sdf);
static SharedPtr Create(const std::string & node_name, sdf::ElementPtr _sdf);

/// Create a #gazebo_ros::Node and add it to the global #gazebo_ros::Executor.
/**
Expand All @@ -95,25 +95,26 @@ class Node : public rclcpp::Node
* \param[in] args List of arguments to pass to <a href="http://docs.ros2.org/latest/api/rclcpp/classrclcpp_1_1_node.html">rclcpp::Node</a>
* \return A shared pointer to a new #gazebo_ros::Node
*/
template <typename ...Args>
static SharedPtr Create(Args && ...args);
template<typename ... Args>
static SharedPtr Create(Args && ... args);

/// Initialize ROS with the command line arguments.
/**
* \note Must be called ONCE before any #gazebo_ros::Node instances are created. Subsequent calls are ignored.
* \param[in] argc Number of arguments
* \param[in] argv Vector of c-strings of length \a argc
*/
static void InitROS(int argc, char** argv);
static void InitROS(int argc, char ** argv);

private:
/// Inherit constructor
/// Inherit constructor
using rclcpp::Node::Node;

/// Points to #static_executor_, so that when all #gazebo_ros::Node instances are destroyed, the executor thread is too
/// Points to #static_executor_, so that when all #gazebo_ros::Node instances are destroyed, the
/// executor thread is too
std::shared_ptr<Executor> executor_;

/// true if #InitROS has been called and future calls will be ignored
/// True if #InitROS has been called and future calls will be ignored
static std::atomic_bool initialized_;

/// Locks #initialized_ and #executor_
Expand All @@ -123,22 +124,22 @@ class Node : public rclcpp::Node
static std::weak_ptr<Executor> static_executor_;
};

template <typename ...Args>
Node::SharedPtr Node::Create(Args && ...args)
template<typename ... Args>
Node::SharedPtr Node::Create(Args && ... args)
{
// Throw exception is Node is created before ROS is initialized
if (!initialized_)
if (!initialized_) {
throw NotInitializedException();
}

// Contruct Node by forwarding arguments
Node::SharedPtr node = std::make_shared<Node>(std::forward<Args>(args)...);
Node::SharedPtr node = std::make_shared<Node>(std::forward<Args>(args) ...);

std::lock_guard<std::mutex> l(lock_);
// Store shared pointer to static executor in this object
node->executor_ = static_executor_.lock();
// If executor has not been contructed yet, do so now
if (!node->executor_)
{
if (!node->executor_) {
node->executor_ = std::make_shared<Executor>();
static_executor_ = node->executor_;
}
Expand All @@ -147,7 +148,5 @@ Node::SharedPtr Node::Create(Args && ...args)
node->executor_->add_node(node);
return node;
}


} // namespace gazebo_ros
#endif
} // namespace gazebo_ros
#endif // GAZEBO_ROS__NODE_HPP_
3 changes: 3 additions & 0 deletions gazebo_ros/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,9 @@
<exec_depend>rclcpp</exec_depend>
<exec_depend>gazebo_dev</exec_depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
13 changes: 7 additions & 6 deletions gazebo_ros/src/executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,24 +12,25 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <iostream>

#include <gazebo_ros/executor.hpp>

#include <iostream>

namespace gazebo_ros
{

Executor::Executor() :
spin_thread_(std::bind(&Executor::run, this))
Executor::Executor()
: spin_thread_(std::bind(&Executor::run, this))
{
sigint_handle_ = gazebo::event::Events::ConnectSigInt(std::bind(&Executor::shutdown, this));
}

Executor::~Executor()
{
// If ros was not already shutdown by SIGINT handler, do it now
if (rclcpp::ok())
if (rclcpp::ok()) {
rclcpp::shutdown();
}
spin_thread_.join();
}

Expand All @@ -43,4 +44,4 @@ void Executor::shutdown()
rclcpp::shutdown();
}

} // namespace gazebo_ros
} // namespace gazebo_ros
23 changes: 13 additions & 10 deletions gazebo_ros/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,38 +14,41 @@

#include <gazebo_ros/node.hpp>

#include <memory>
#include <string>

namespace gazebo_ros
{

std::weak_ptr<Executor> Node::static_executor_;
std::mutex Node::lock_;
std::atomic_bool Node::initialized_(false);

Node::NotInitializedException::NotInitializedException() :
std::runtime_error("Called LoadNode before InitROS. Be sure to load the ros_api plugin first")
Node::NotInitializedException::NotInitializedException()
: std::runtime_error("Called LoadNode before InitROS. Be sure to load the ros_api plugin first")
{
}

Node::~Node()
{
}

void Node::InitROS(int argc, char** argv)
void Node::InitROS(int argc, char ** argv)
{
std::lock_guard<std::mutex> l(lock_);
if (initialized_)
{
RCLCPP_WARN(rclcpp::get_logger("gazebo_ros_node"), "Called Node::InitROS twice, ignoring second call.");
std::lock_guard<std::mutex> l(lock_);
if (initialized_) {
RCLCPP_WARN(rclcpp::get_logger("gazebo_ros_node"),
"Called Node::InitROS twice, ignoring second call.");
return;
}

rclcpp::init(argc, argv);
initialized_ = true;
}

Node::SharedPtr Node::Create(const std::string& node_name)
Node::SharedPtr Node::Create(const std::string & node_name)
{
return Create<const std::string&>(node_name);
return Create<const std::string &>(node_name);
}

} // namespace gazebo_ros
} // namespace gazebo_ros

0 comments on commit dee249e

Please sign in to comment.