Added a package to handle the CM10 buttons through dynamic reconfigure.

Added specific config and launch files for the CM510 simulation.

bioloid_apps/launch/ceabot/ceabot_base.launch

+<launch>
+
+  <arg name="robot" default="bioloid_ceabot" />
+
+  <include file="$(find bioloid_description)/launch/bioloid_cm510_sim.launch">
+    <arg name="robot" value="$(arg robot)" />
+  </include>
+
+  <!-- launch the action client node -->
+  <node name="cm510_buttons"
+        pkg="bioloid_cm510_buttons"
+        type="bioloid_cm510_buttons"
+        output="screen"
+        ns="/bioloid">
+    <remap from="/bioloid/cm510_buttons/buttons"
+             to="/bioloid/sensors/pushbuttons"/>
+  </node>
+
+  <!-- launch dynamic reconfigure -->
+  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+    output="screen"/>
+
+</launch>
+

bioloid_cm510_buttons/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(bioloid_cm510_buttons)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ******************************************************************** 
+#                 Add catkin additional components here
+# ******************************************************************** 
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_msgs)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ******************************************************************** 
+#           Add system and labrobotica dependencies here
+# ******************************************************************** 
+# find_package(<dependency> REQUIRED)
+
+# ******************************************************************** 
+#           Add topic, service and action definition here
+# ******************************************************************** 
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+# ******************************************************************** 
+#                 Add the dynamic reconfigure file 
+# ******************************************************************** 
+generate_dynamic_reconfigure_options(cfg/BioloidCm510Buttons.cfg)
+
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+#  INCLUDE_DIRS 
+#  LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+ CATKIN_DEPENDS iri_base_algorithm std_msgs
+# ******************************************************************** 
+#      Add system and labrobotica run time dependencies here
+# ******************************************************************** 
+#  DEPENDS 
+)
+
+###########
+## Build ##
+###########
+
+# ******************************************************************** 
+#                   Add the include directories 
+# ******************************************************************** 
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/bioloid_cm510_buttons_alg.cpp src/bioloid_cm510_buttons_alg_node.cpp)
+
+# ******************************************************************** 
+#                   Add the libraries
+# ******************************************************************** 
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ******************************************************************** 
+#               Add message headers dependencies 
+# ******************************************************************** 
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
+# ******************************************************************** 
+#               Add dynamic reconfigure dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})

bioloid_cm510_buttons/cfg/BioloidCm510Buttons.cfg

+#! /usr/bin/env python
+#*  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 the Willow Garage 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.
+#***********************************************************
+
+# Author: 
+
+PACKAGE='bioloid_cm510_buttons'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
+#gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+gen.add("start_button",            bool_t,    0,                               "CM510 start button",             False)
+gen.add("up_button",               bool_t,    0,                               "CM510 up button",                False)
+gen.add("down_button",             bool_t,    0,                               "CM510 down button",              False)
+gen.add("left_button",             bool_t,    0,                               "CM510 left button",              False)
+gen.add("right_button",            bool_t,    0,                               "CM510 right button",             False)
+
+exit(gen.generate(PACKAGE, "BioloidCm510ButtonsAlgorithm", "BioloidCm510Buttons"))

bioloid_cm510_buttons/include/bioloid_cm510_buttons_alg.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _bioloid_cm510_buttons_alg_h_
+#define _bioloid_cm510_buttons_alg_h_
+
+#include <bioloid_cm510_buttons/BioloidCm510ButtonsConfig.h>
+
+//include bioloid_cm510_buttons_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class BioloidCm510ButtonsAlgorithm
+{
+  protected:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the BioloidCm510ButtonsConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    pthread_mutex_t access_;    
+
+    // private attributes and methods
+
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the BioloidCm510ButtonsConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef bioloid_cm510_buttons::BioloidCm510ButtonsConfig Config;
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+
+   /**
+    * \brief constructor
+    *
+    * In this constructor parameters related to the specific driver can be
+    * initalized. Those parameters can be also set in the openDriver() function.
+    * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+    * may be also overload here.
+    */
+    BioloidCm510ButtonsAlgorithm(void);
+
+   /**
+    * \brief Lock Algorithm
+    *
+    * Locks access to the Algorithm class
+    */
+    void lock(void) { pthread_mutex_lock(&this->access_); };
+
+   /**
+    * \brief Unlock Algorithm
+    *
+    * Unlocks access to the Algorithm class
+    */
+    void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+   /**
+    * \brief Tries Access to Algorithm
+    *
+    * Tries access to Algorithm
+    * 
+    * \return true if the lock was adquired, false otherwise
+    */
+    bool try_enter(void) 
+    { 
+      if(pthread_mutex_trylock(&this->access_)==0)
+        return true;
+      else
+        return false;
+    };
+
+   /**
+    * \brief config update
+    *
+    * In this function the driver parameters must be updated with the input
+    * config variable. Then the new configuration state will be stored in the 
+    * Config attribute.
+    *
+    * \param new_cfg the new driver configuration state
+    *
+    * \param level level in which the update is taken place
+    */
+    void config_update(Config& config, uint32_t level=0);
+
+    // here define all bioloid_cm510_buttons_alg interface methods to retrieve and set
+    // the driver parameters
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~BioloidCm510ButtonsAlgorithm(void);
+};
+
+#endif

bioloid_cm510_buttons/include/bioloid_cm510_buttons_alg_node.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _bioloid_cm510_buttons_alg_node_h_
+#define _bioloid_cm510_buttons_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "bioloid_cm510_buttons_alg.h"
+
+// [publisher subscriber headers]
+#include <std_msgs/Char.h>
+
+// [service client headers]
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class BioloidCm510ButtonsAlgNode : public algorithm_base::IriBaseAlgorithm<BioloidCm510ButtonsAlgorithm>
+{
+  private:
+    // [publisher attributes]
+    ros::Publisher buttons_publisher_;
+    std_msgs::Char buttons_Char_msg_;
+
+
+    // [subscriber attributes]
+
+    // [service attributes]
+
+    // [client attributes]
+
+    // [action server attributes]
+
+    // [action client attributes]
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+  public:
+   /**
+    * \brief Constructor
+    * 
+    * This constructor initializes specific class attributes and all ROS
+    * communications variables to enable message exchange.
+    */
+    BioloidCm510ButtonsAlgNode(void);
+
+   /**
+    * \brief Destructor
+    * 
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~BioloidCm510ButtonsAlgNode(void);
+
+  protected:
+   /**
+    * \brief main node thread
+    *
+    * This is the main thread node function. Code written here will be executed
+    * in every node loop while the algorithm is on running state. Loop frequency 
+    * can be tuned by modifying loop_rate attribute.
+    *
+    * Here data related to the process loop or to ROS topics (mainly data structs
+    * related to the MSG and SRV files) must be updated. ROS publisher objects 
+    * must publish their data in this process. ROS client servers may also
+    * request data to the corresponding server topics.
+    */
+    void mainNodeThread(void);
+
+   /**
+    * \brief dynamic reconfigure server callback
+    * 
+    * This method is called whenever a new configuration is received through
+    * the dynamic reconfigure. The derivated generic algorithm class must 
+    * implement it.
+    *
+    * \param config an object with new configuration from all algorithm 
+    *               parameters defined in the config file.
+    * \param level  integer referring the level in which the configuration
+    *               has been changed.
+    */
+    void node_config_update(Config &config, uint32_t level);
+
+   /**
+    * \brief node add diagnostics
+    *
+    * In this abstract function additional ROS diagnostics applied to the 
+    * specific algorithms may be added.
+    */
+    void addNodeDiagnostics(void);
+
+    // [diagnostic functions]
+    
+    // [test functions]
+};
+
+#endif

bioloid_cm510_buttons/package.xml

+<?xml version="1.0"?>
+<package>
+  <name>bioloid_cm510_buttons</name>
+  <version>0.0.0</version>
+  <description>The bioloid_cm510_buttons package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="sergi@todo.todo">sergi</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>LGPL</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/bioloid_cm510_buttons</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>iri_base_algorithm</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>iri_base_algorithm</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
\ No newline at end of file

bioloid_cm510_buttons/src/bioloid_cm510_buttons_alg.cpp

+#include "bioloid_cm510_buttons_alg.h"
+
+BioloidCm510ButtonsAlgorithm::BioloidCm510ButtonsAlgorithm(void)
+{
+  pthread_mutex_init(&this->access_,NULL);
+}
+
+BioloidCm510ButtonsAlgorithm::~BioloidCm510ButtonsAlgorithm(void)
+{
+  pthread_mutex_destroy(&this->access_);
+}
+
+void BioloidCm510ButtonsAlgorithm::config_update(Config& config, uint32_t level)
+{
+  this->lock();
+
+  // save the current configuration
+  this->config_=config;
+  
+  this->unlock();
+}
+
+// BioloidCm510ButtonsAlgorithm Public API

bioloid_cm510_buttons/src/bioloid_cm510_buttons_alg_node.cpp

+#include "bioloid_cm510_buttons_alg_node.h"
+
+BioloidCm510ButtonsAlgNode::BioloidCm510ButtonsAlgNode(void) :
+  algorithm_base::IriBaseAlgorithm<BioloidCm510ButtonsAlgorithm>()
+{
+  //init class attributes if necessary
+  //this->loop_rate_ = 2;//in [Hz]
+
+  // [init publishers]
+  this->buttons_publisher_ = this->public_node_handle_.advertise<std_msgs::Char>("buttons", 1);
+  
+  // [init subscribers]
+  
+  // [init services]
+  
+  // [init clients]
+  
+  // [init action servers]
+  
+  // [init action clients]
+}
+
+BioloidCm510ButtonsAlgNode::~BioloidCm510ButtonsAlgNode(void)
+{
+  // [free dynamic memory]
+}
+
+void BioloidCm510ButtonsAlgNode::mainNodeThread(void)
+{
+  // [fill msg structures]
+  // Initialize the topic message structure
+  //this->buttons_Char_msg_.data = my_var;
+
+  
+  // [fill srv structure and make request to the server]
+  
+  // [fill action structure and make request to the action server]
+
+  // [publish messages]
+
+}
+
+/*  [subscriber callbacks] */
+
+/*  [service callbacks] */
+
+/*  [action callbacks] */
+
+/*  [action requests] */
+
+void BioloidCm510ButtonsAlgNode::node_config_update(Config &config, uint32_t level)
+{
+  unsigned char byte=0x00;
+
+  this->alg_.lock();
+  this->config_=config;
+  if(config.start_button)
+    byte|=0x01;
+  else
+    byte&=0xFE;
+  if(config.up_button)
+    byte|=0x10;
+  else
+    byte&=0xEF;
+  if(config.down_button)
+    byte|=0x20;
+  else
+    byte&=0xDF;
+  if(config.left_button)
+    byte|=0x40;
+  else
+    byte&=0xBF;
+  if(config.right_button)
+    byte|=0x80;
+  else
+    byte&=0x7F;
+  this->buttons_Char_msg_.data=byte;
+  // Uncomment the following line to publish the topic message
+  this->buttons_publisher_.publish(this->buttons_Char_msg_);
+  this->alg_.unlock();
+}
+
+void BioloidCm510ButtonsAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+  return algorithm_base::main<BioloidCm510ButtonsAlgNode>(argc, argv, "bioloid_cm510_buttons_alg_node");
+}

bioloid_control/config/bioloid_cm510_control.yaml

+bioloid:
+  # Publish all joint states -----------------------------------
+  joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50
+
+  bioloid_cm510_cont:
+    type: effort_controllers/BioloidControllerCM510
+    adc1_frame: IR1_link
+    adc3_frame: gyro_x
+    adc4_frame: gyro_y
+    exp_adc1_frame: IR2_link
+    exp_board_id: 192
+    joints:
+      - j_shoulder_r
+      - j_shoulder_l
+      - j_high_arm_r
+      - j_high_arm_l
+      - j_low_arm_r
+      - j_low_arm_l
+      - j_pelvis_yaw_r
+      - j_pelvis_yaw_l
+      - j_pelvis_roll_r
+      - j_pelvis_roll_l
+      - j_pelvis_pitch_r
+      - j_pelvis_pitch_l
+      - j_knee_r
+      - j_knee_l
+      - j_ankle_pitch_r
+      - j_ankle_pitch_l
+      - j_ankle_roll_r
+      - j_ankle_roll_l
+    gains:
+      j_shoulder_l:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_high_arm_l:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_low_arm_l:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_shoulder_r:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_high_arm_r:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_low_arm_r:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_pelvis_yaw_l:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_pelvis_roll_l:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_pelvis_pitch_l:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_knee_l:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_ankle_pitch_l:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_ankle_roll_l:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_pelvis_yaw_r:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_pelvis_roll_r:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_pelvis_pitch_r:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_knee_r:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_ankle_pitch_r:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5
+      j_ankle_roll_r:
+        p: 8.0
+        d: 0.0
+        i: 0.0
+        proxy:
+          lambda: 3.0
+          vel_limit: 6.0
+          effort_limit: 1.5

bioloid_control/config/bioloid_control.yaml

@@ -5,12 +5,7 @@ bioloid:
     publish_rate: 50
 
   bioloid_cont:
-    type: effort_controllers/BioloidControllerCM510
-    adc1_frame: IR1_link
-    adc3_frame: gyro_x
-    adc4_frame: gyro_y
-    exp_adc1_frame: IR2_link
-    exp_board_id: 192
+    type: effort_controllers/BioloidController
     joints:
       - j_shoulder_r
       - j_shoulder_l

bioloid_control/launch/bioloid_cm510_control.launch

+<launch>
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find bioloid_control)/config/bioloid_cm510_control.yaml" command="load"/>
+
+  <param name="motions_file" value="$(find bioloid_controller_cm510)/motions/bio_prm_humanoidtypea_en.mtn"/>
+
+  <!-- load the controllers -->
+  <node name="controller_spawner" 
+        pkg="controller_manager" 
+        type="spawner" 
+        respawn="false"
+        output="screen" 
+        ns="/bioloid" 
+        args="joint_state_controller bioloid_cm510_cont">
+   </node>
+
+</launch>
+

bioloid_control/launch/bioloid_control.launch

@@ -3,8 +3,6 @@
   <!-- Load joint controller configurations from YAML file to parameter server -->
   <rosparam file="$(find bioloid_control)/config/bioloid_control.yaml" command="load"/>
 
-  <param name="motions_file" value="$(find bioloid_controller_cm510)/motions/bio_prm_humanoidtypea_en.mtn"/>
-
   <!-- load the controllers -->
   <node name="controller_spawner" 
         pkg="controller_manager" 

bioloid_controller_cm510/include/bioloid_controller_cm510.h

@@ -46,7 +46,7 @@
 #include "std_msgs/Int32.h"
 #include "sensor_msgs/Imu.h"
 #include "sensor_msgs/Range.h"
-#include "std_msgs/Byte.h"
+#include "std_msgs/Char.h"
 
 // URDF
 #include <urdf/model.h>
@@ -120,7 +120,7 @@ namespace bioloid_controller_cm510
       ros::Subscriber range_sub;
       void range_callback(const sensor_msgs::Range::ConstPtr& msg);
       ros::Subscriber pushbuttons_sub;
-      void pushbuttons_callback(const std_msgs::Byte::ConstPtr& msg);
+      void pushbuttons_callback(const std_msgs::Char::ConstPtr& msg);
 
       // ROS API
       ros::NodeHandle controller_nh_;

bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h

@@ -431,7 +431,7 @@ namespace bioloid_controller_cm510
     }
 
   template <class HardwareInterface>
-    void BioloidControllerCM510<HardwareInterface>::pushbuttons_callback(const std_msgs::Byte::ConstPtr& msg)
+    void BioloidControllerCM510<HardwareInterface>::pushbuttons_callback(const std_msgs::Char::ConstPtr& msg)
     {
       if(msg->data&BTN_UP)
         PINE&=~BTN_UP;

bioloid_description/launch/bioloid_cm510_sim.launch

+<launch>
+
+  <arg name="robot" default="bioloid_ceabot" />
+
+  <include file="$(find bioloid_gazebo)/launch/bioloid_gazebo.launch">
+    <arg name="robot" value="$(arg robot)"/>
+  </include>
+
+  <include file="$(find bioloid_control)/launch/bioloid_cm510_control.launch"/>
+
+</launch>

bioloid_robot/package.xml

@@ -45,6 +45,7 @@
   <run_depend>bioloid_control</run_depend> 
   <run_depend>bioloid_controller</run_depend> 
   <run_depend>bioloid_controller_cm510</run_depend> 
+  <run_depend>bioloid_cm510_buttons</run_depend> 
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>