* Add yujin command velocity multiplexer source code.

CHANGELOG.rst

@@ -0,0 +1,83 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package yocs_cmd_vel_mux
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.6.3 (2014-12-05)
+------------------
+
+0.6.2 (2014-11-30)
+------------------
+* yocs_cmd_vel_mux: fixes node handle for output pub
+ to keep backwards compatibility
+* adds a little launcher restructing for muxer and smoother
+* Contributors: Marcus Liebhardt
+
+0.6.0 (2014-07-08)
+------------------
+* updating package informations. remove email for authors. updating maintainer
+* Contributors: Jihoon Lee
+
+0.5.3 (2014-03-24)
+------------------
+* Added support for YAML-CPP 0.5+.
+ The new yaml-cpp API removes the "node >> outputvar;" operator, and
+ it has a new way of loading documents. There's no version hint in the
+ library's headers, so I'm getting the version number from pkg-config.
+ This part of the patch is a port of the ones created by @ktossell for
+ map_server and other packages.
+ The new yaml-cpp also does not have FindValue.
+* Contributors: Scott K Logan
+
+0.5.2 (2013-11-05)
+------------------
+
+0.5.1 (2013-10-14)
+------------------
+* Unify naming politics for binaries and plugins.
+
+0.5.0 (2013-10-11)
+------------------
+* Renamed as yocs_cmd_vel_mux.
+
+0.4.1 (2013-10-08)
+------------------
+
+0.4.0 (2013-08-29)
+------------------
+* Add bugtracker and repo info URLs.
+* Changelogs at package level.
+* License link fixed.
+
+0.3.0 (2013-07-02)
+------------------
+
+0.2.3 (2013-04-15)
+------------------
+
+0.2.2 (2013-02-10)
+------------------
+
+0.2.1 (2013-02-08)
+------------------
+
+0.2.0 (2013-02-07)
+------------------
+* Catkinized.
+
+0.1.3 (2013-01-08)
+------------------
+* More generous description.
+
+0.1.2 (2013-01-02)
+------------------
+* Dynamically reconfigurable.
+* Upgraded to new groovy plugin formats.
+* Add reconfigure launcher and parameter file.
+* Add a dynamic reconfigure script to accept a yaml filename.
+
+0.1.1 (2012-12-21)
+------------------
+
+0.1.0 (2012-12-05)
+------------------
+* Initial version.

CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(yocs_cmd_vel_mux)
+find_package(catkin REQUIRED COMPONENTS roscpp pluginlib nodelet dynamic_reconfigure geometry_msgs)
+
+# pkg-config support
+find_package(PkgConfig)
+pkg_search_module(yaml-cpp REQUIRED yaml-cpp)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+if(NOT ${yaml-cpp_VERSION} VERSION_LESS "0.5")
+add_definitions(-DHAVE_NEW_YAMLCPP)
+endif()
+
+# Dynamic reconfigure support
+generate_dynamic_reconfigure_options(cfg/reload.cfg)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}_nodelet
+ CATKIN_DEPENDS roscpp pluginlib nodelet dynamic_reconfigure geometry_msgs
+ DEPENDS yaml-cpp
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${yaml-cpp_INCLUDE_DIRS})
+
+# Nodelet library
+add_library(${PROJECT_NAME}_nodelet src/cmd_vel_mux_nodelet.cpp src/cmd_vel_subscribers.cpp)
+add_dependencies(${PROJECT_NAME}_nodelet geometry_msgs_gencpp)
+add_dependencies(${PROJECT_NAME}_nodelet ${PROJECT_NAME}_gencfg)
+
+target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES} ${yaml-cpp_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME}_nodelet
+ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+install(DIRECTORY plugins
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+install(DIRECTORY launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+install(DIRECTORY param
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

LICENSE

@@ -0,0 +1,31 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012 Yujin Robot, Daniel Stonier, Jorge Santos
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of Yujin Robot nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.

cfg/reload.cfg

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+PACKAGE = "yocs_cmd_vel_mux"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("yaml_cfg_file", str_t, 0, "Pathname to a yaml file for re-configuration of the mux", "")
+gen.add("yaml_cfg_data", str_t, 0, "Yaml-formatted string for re-configuration of the mux", "")
+
+# Second arg is node name it will run in (doc purposes only), third is generated filename prefix
+exit(gen.generate(PACKAGE, "yocs_cmd_vel_mux_reload", "reload"))

include/yocs_cmd_vel_mux/cmd_vel_mux_nodelet.hpp

@@ -0,0 +1,119 @@
+/**
+ * @file /include/yocs_cmd_vel_mux/cmd_vel_mux_nodelet.hpp
+ *
+ * @brief Structure for the yocs_cmd_vel_mux.
+ *
+ * License: BSD
+ * https://raw.github.com/yujinrobot/yujin_ocs/hydro/yocs_cmd_vel_mux/LICENSE
+ **/
+/*****************************************************************************
+ ** Ifdefs
+ *****************************************************************************/
+
+#ifndef YUJIN_OCS_CMD_VEL_MUX_HPP_
+#define YUJIN_OCS_CMD_VEL_MUX_HPP_
+
+/*****************************************************************************
+ ** Includes
+ *****************************************************************************/
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <dynamic_reconfigure/server.h>
+
+#include "yocs_cmd_vel_mux/reloadConfig.h"
+#include "yocs_cmd_vel_mux/cmd_vel_subscribers.hpp"
+
+/*****************************************************************************
+** Namespaces
+*****************************************************************************/
+
+namespace yocs_cmd_vel_mux {
+
+/*****************************************************************************
+ ** CmdVelMux
+ *****************************************************************************/
+
+class CmdVelMuxNodelet : public nodelet::Nodelet
+{
+public:
+ virtual void onInit();
+
+ CmdVelMuxNodelet()
+ {
+ cmd_vel_subs.allowed = VACANT;
+ dynamic_reconfigure_server = NULL;
+ }
+
+ ~CmdVelMuxNodelet()
+ {
+ if (dynamic_reconfigure_server != NULL)
+ delete dynamic_reconfigure_server;
+ }
+
+private:
+ static const unsigned int VACANT = 666666; /**< ID for "nobody" active input; anything big is ok */
+ static const unsigned int GLOBAL_TIMER = 888888; /**< ID for the global timer functor; anything big is ok */
+
+ CmdVelSubscribers cmd_vel_subs; /**< Pool of cmd_vel topics subscribers */
+ ros::Publisher output_topic_pub; /**< Multiplexed command velocity topic */
+ std::string output_topic_name; /**< Multiplexed command velocity topic name */
+ ros::Publisher active_subscriber; /**< Currently allowed cmd_vel subscriber */
+ ros::Timer common_timer; /**< No messages from any subscriber timeout */
+ double common_timer_period; /**< No messages from any subscriber timeout period */
+
+ void timerCallback(const ros::TimerEvent& event, unsigned int idx);
+ void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg, unsigned int idx);
+
+ /*********************
+ ** Dynamic Reconfigure
+ **********************/
+ dynamic_reconfigure::Server<yocs_cmd_vel_mux::reloadConfig> * dynamic_reconfigure_server;
+ dynamic_reconfigure::Server<yocs_cmd_vel_mux::reloadConfig>::CallbackType dynamic_reconfigure_cb;
+ void reloadConfiguration(yocs_cmd_vel_mux::reloadConfig &config, uint32_t unused_level);
+
+ /*********************
+ ** Private Classes
+ **********************/
+ // Functor assigned to each incoming velocity topic to bind it to cmd_vel callback
+ class CmdVelFunctor
+ {
+ private:
+ unsigned int idx;
+ CmdVelMuxNodelet* node;
+
+ public:
+ CmdVelFunctor(unsigned int idx, CmdVelMuxNodelet* node) :
+ idx(idx), node(node)
+ {
+ }
+
+ void operator()(const geometry_msgs::Twist::ConstPtr& msg)
+ {
+ node->cmdVelCallback(msg, idx);
+ }
+ };
+
+ // Functor assigned to each velocity messages source to bind it to timer callback
+ class TimerFunctor
+ {
+ private:
+ unsigned int idx;
+ CmdVelMuxNodelet* node;
+
+ public:
+ TimerFunctor(unsigned int idx, CmdVelMuxNodelet* node) :
+ idx(idx), node(node)
+ {
+ }
+
+ void operator()(const ros::TimerEvent& event)
+ {
+ node->timerCallback(event, idx);
+ }
+ };
+};
+
+} // namespace yocs_cmd_vel_mux
+
+#endif /* YUJIN_OCS_CMD_VEL_MUX_HPP_ */