diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0dd0b4000dd89bedd1d7ed158260bba30a33d93b..887093d2b5fb651c23de33fa68853d1bb0ad0c93 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
-project(darwin_controller_cpp)
+project(dynamixel_robot_gazebo)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -11,7 +11,6 @@ find_package(catkin REQUIRED roscpp urdf controller_interface hardware_interface
FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(comm REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
-FIND_PACKAGE(bno055_imu_sim REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
@@ -92,80 +91,42 @@ catkin_package(
link_directories(${GAZEBO_LIBRARY_DIRS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
-SET(DARWIN_FW_PATH ~/humanoids/darwin_stm32_fw)
-SET(STM32_HAL_PATH ~/humanoids/stm32_hal)
-SET(STM32_LIBRARIES_PATH ~/humanoids/stm32_libraries)
-
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
-include_directories(include include/sim ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
-include_directories(${DARWIN_FW_PATH}/include/)
-include_directories(${STM32_LIBRARIES_PATH}/dynamixel_base/include)
-include_directories(${STM32_LIBRARIES_PATH}/comm/include)
-include_directories(${STM32_LIBRARIES_PATH}/f1/usart/include)
-include_directories(${STM32_LIBRARIES_PATH}/utils/include)
-include_directories(${STM32_HAL_PATH}/f1/include)
-include_directories(${STM32_HAL_PATH}/f1/include/core)
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
include_directories(${iriutils_INCLUDE_DIR})
include_directories(${comm_INCLUDE_DIR})
include_directories(${dynamixel_INCLUDE_DIR})
-include_directories(${bno055_imu_sim_INCLUDE_DIR})
-
-add_definitions(-DSTM32F103xE)
-add_definitions(-DUSE_HAL_DRIVER)
## Declare a cpp library
-# add_library(darwin_controller
-# src/${PROJECT_NAME}/darwin_controller.cpp
+# add_library(dynamixel_controller
+# src/${PROJECT_NAME}/dynamixel_controller.cpp
# )
-add_library(${PROJECT_NAME} src/darwin_controller_cpp.cpp
- ${DARWIN_FW_PATH}/src/ram.c
- ${DARWIN_FW_PATH}/src/action.c
- ${DARWIN_FW_PATH}/src/walking.c
- ${DARWIN_FW_PATH}/src/darwin_kinematics.c
- ${DARWIN_FW_PATH}/src/darwin_math.c
- ${DARWIN_FW_PATH}/src/motion_pages.c
- ${DARWIN_FW_PATH}/src/joint_motion.c
- ${DARWIN_FW_PATH}/src/head_tracking.c
- ${DARWIN_FW_PATH}/src/motion_manager.c
- ${DARWIN_FW_PATH}/src/smart_charger.c
- ${DARWIN_FW_PATH}/src/grippers.c
- ${DARWIN_FW_PATH}/src/gpio.c
- ${DARWIN_FW_PATH}/src/stairs.c
- ${DARWIN_FW_PATH}/src/adc_dma.c
- src/sim/stm32_hal.c
- src/sim/stm32_cortex.c
- src/sim/stm32_gpio.c
- src/sim/stm32_timer.c
- src/sim/stm32_timer_ex.c
- src/sim/stm32_dma.c
- src/sim/stm32_dyn_master.c
- src/sim/stm32_dyn_slave.c
- src/sim/stm32_dyn_master_servos.c
- src/sim/stm32_dyn_master_v2_servos.c
- src/sim/stm32_eeprom.c
- src/sim/imu.c
- include/darwin_controller_cpp.h
- include/darwin_controller_cpp_impl.h)
+add_library(${PROJECT_NAME} src/dynamixel_robot_gazebo.cpp
+ src/dynamixel_servo.cpp)
## Declare a cpp executable
-# add_executable(darwin_controller_node src/darwin_controller_node.cpp)
+# add_executable(dynamixel_controller_node src/dynamixel_controller_node.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
-add_dependencies(darwin_controller_cpp humanoid_common_msgs_generate_messages_cpp)
+# add_dependencies(dynamixel_robot_gazebo humanoid_common_msgs_generate_messages_cpp)
## Specify libraries to link a library or executable target against
-# target_link_libraries(darwin_controller_node
+# target_link_libraries(dynamixel_controller_node
# ${catkin_LIBRARIES}
# )
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${comm_LIBRARY})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${dynamixel_LIBRARY})
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${bno055_imu_sim_LIBRARY})
+add_custom_command(
+ TARGET ${PROJECT_NAME} POST_BUILD
+ COMMAND ${CMAKE_COMMAND} -E copy
+ ${CMAKE_CURRENT_SOURCE_DIR}/scripts/start_socat.sh
+ ${CATKIN_PACKAGE_LIB_DESTINATION}/start_socat.sh)
#############
## Install ##
#############
@@ -176,12 +137,12 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${bno055_imu_sim_LIBRARY})
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
-# scripts/my_python_script
+# scripts/start_socat.sh
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
-# install(TARGETS darwin_controller darwin_controller_node
+# install(TARGETS dynamixel_controller dynamixel_controller_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -206,7 +167,7 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${bno055_imu_sim_LIBRARY})
#############
## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_darwin_controller.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_dynamixel_controller.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
diff --git a/dynamixel_robot_plugin.xml b/dynamixel_robot_plugin.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2100cde095beb12b26aa6477cd0ac273c35b9283
--- /dev/null
+++ b/dynamixel_robot_plugin.xml
@@ -0,0 +1,12 @@
+<library path="lib/libdynamixel_robot_gazebo">
+
+ <class name="dynamixel_robot_gazebo/DynamixelRobotGazebo"
+ type="dynamixel_robot_gazebo::DynamixelRobotGazebo"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ this is a description
+ </description>
+ </class>
+
+</library>
+
diff --git a/include/darwin_controller_cpp_impl.h b/include/darwin_controller_cpp_impl.h
deleted file mode 100644
index e10277b25955849a46006cbf448c51d5e3ee4b9b..0000000000000000000000000000000000000000
--- a/include/darwin_controller_cpp_impl.h
+++ /dev/null
@@ -1,380 +0,0 @@
-#ifndef DARWIN_CONTROLLER_CPP_IMP_H
-#define DARWIN_CONTROLLER_CPP_IMP_H
-
-#include "stm32f1xx.h"
-#include "motion_manager.h"
-#include "action.h"
-#include "action_id.h"
-#include "walking.h"
-#include "joint_motion.h"
-#include "head_tracking.h"
-#include "grippers.h"
-#include "ram.h"
-#include "imu.h"
-#include "darwin_math.h"
-#include "stairs.h"
-#include "darwin_dyn_slave.h"
-
-#ifdef _HAVE_XSD
-#include "../src/xml/darwin_config.hxx"
-#endif
-
-#include "exceptions.h"
-
-extern "C" {
- // motion_manager private function declaration
- void TIM5_IRQHandler(void);
-}
-
-// motion manager external variables
-extern int num_servos;
-extern double real_angles[PAGE_MAX_NUM_SERVOS];
-
-// action module external variables
-extern TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
-
-//IMU module external variables
-extern int32_t gyro_data[3];
-extern int32_t accel_data[3];
-
-// dynamixel slave external variables
-extern "C" {
- extern void darwin_on_ping(void);
- extern unsigned char darwin_on_read(unsigned short int address, unsigned short int length, unsigned char *data);
- extern unsigned char darwin_on_write(unsigned short int address, unsigned short int length, unsigned char *data);
-}
-
-//parameters default values
-#define DEFAULT_ENABLE_SMART_CHARGER false
-#define DEFAULT_INITIAL_CHARGE 2200.0
-#define DEFAULT_DISCHARGE_RATE 6000.0
-#define DEFAULT_CHARGE_CURRENT 2200.0
-#define DEFAULT_CHARGE_STATION_LINK "none"
-#define DEFAULT_CHARGE_ROBOT_LINK "none"
-
-namespace darwin_controller_cpp
-{
- namespace internal
- {
- std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string& param_name)
- {
- using namespace XmlRpc;
- XmlRpcValue xml_array;
- if (!nh.getParam(param_name, xml_array))
- {
- ROS_ERROR_STREAM("CDarwinSim : Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
- return std::vector<std::string>();
- }
- if (xml_array.getType() != XmlRpcValue::TypeArray)
- {
- ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter is not an array (namespace: " <<
- nh.getNamespace() << ").");
- return std::vector<std::string>();
- }
-
- std::vector<std::string> out;
- for (int i = 0; i < xml_array.size(); ++i)
- {
- if (xml_array[i].getType() != XmlRpcValue::TypeString)
- {
- ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter contains a non-string element (namespace: " <<
- nh.getNamespace() << ").");
- return std::vector<std::string>();
- }
- out.push_back(static_cast<std::string>(xml_array[i]));
- }
- return out;
- }
-
- boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
- {
- boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
-
- std::string urdf_str;
- // Check for robot_description in proper namespace
- if (nh.getParam(param_name, urdf_str))
- {
- if (!urdf->initString(urdf_str))
- {
- ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
- nh.getNamespace() << ").");
- return boost::shared_ptr<urdf::Model>();
- }
- }
- // Check for robot_description in root
- else if (!urdf->initParam("robot_description"))
- {
- ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter");
- return boost::shared_ptr<urdf::Model>();
- }
- return urdf;
- }
-
- typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
- std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
- {
- std::vector<UrdfJointConstPtr> out;
- for (unsigned int i = 0; i < joint_names.size(); ++i)
- {
- UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
- if (urdf_joint)
- {
- out.push_back(urdf_joint);
- }
- else
- {
- ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names[i] << "' in URDF model.");
- return std::vector<UrdfJointConstPtr>();
- }
- }
- return out;
- }
-
- std::string getLeafNamespace(const ros::NodeHandle& nh)
- {
- const std::string complete_ns = nh.getNamespace();
- std::size_t id = complete_ns.find_last_of("/");
- return complete_ns.substr(id + 1);
- }
-
- } // namespace
-
- template <class HardwareInterface>
- inline void DarwinControllerCPP<HardwareInterface>::starting(const ros::Time& time)
- {
- for(unsigned int i = 0; i <this->joints_.size(); ++i)
- {
- pids_[i]->reset();
- this->joints_[i].setCommand(0.0);
- }
- }
-
- template <class HardwareInterface>
- inline void DarwinControllerCPP<HardwareInterface>::stopping(const ros::Time& time)
- {
- }
-
- template <class HardwareInterface>
- DarwinControllerCPP<HardwareInterface>::DarwinControllerCPP()
- {
- this->dyn_slave=NULL;
- this->imu_sim=NULL;
- }
-
- template <class HardwareInterface>
- bool DarwinControllerCPP<HardwareInterface>::init(HardwareInterface* hw,ros::NodeHandle& root_nh,ros::NodeHandle& controller_nh)
- {
- using namespace internal;
- std::string serial_device;
-
- // Cache controller node handle
- controller_nh_ = controller_nh;
-
- // initialize topics
- this->gyro_sub=root_nh.subscribe("sensors/raw_gyro", 1, &DarwinControllerCPP<HardwareInterface>::gyro_callback,this);
- this->accel_sub=root_nh.subscribe("sensors/raw_accel", 1, &DarwinControllerCPP<HardwareInterface>::accel_callback,this);
- this->imu_sub=root_nh.subscribe("sensors/raw_imu", 1, &DarwinControllerCPP<HardwareInterface>::imu_callback,this);
-
- // Controller name
- name_=getLeafNamespace(controller_nh_);
-
- // List of controlled joints
- joint_names_=getStrings(controller_nh_,"joints");
- if(joint_names_.empty())
- return false;
- const unsigned int n_joints=joint_names_.size();
-
- // URDF joints
- boost::shared_ptr<urdf::Model> urdf=getUrdf(root_nh, "robot_description");
- if(!urdf)
- return false;
-
- std::vector<UrdfJointConstPtr> urdf_joints=getUrdfJoints(*urdf, joint_names_);
- if(urdf_joints.empty())
- return false;
- assert(n_joints==urdf_joints.size());
- num_servos=n_joints;// set the number of servos to the number of joint of the urdf model
-
- // Initialize members
- this->joints_.resize(n_joints);
- this->pids_.resize(n_joints);
- for(unsigned int i=0;i<n_joints;++i)
- {
- // Joint handle
- try {
- joints_[i]=hw->getHandle(joint_names_[i]);
- }catch(...){
- ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names_[i] << "' in '" <<
- this->getHardwareInterfaceType() << "'.");
- return false;
- }
- // Node handle to PID gains
- ros::NodeHandle joint_nh(this->controller_nh_,std::string("gains/")+joint_names_[i]);
-
- // Init PID gains from ROS parameter server
- this->pids_[i].reset(new control_toolbox::Pid());
- if(!this->pids_[i]->init(joint_nh))
- {
- ROS_WARN_STREAM("CDarwinSim : Failed to initialize PID gains from ROS parameter server.");
- return false;
- }
-
- // Whether a joint is continuous (ie. has angle wraparound)
- const std::string not_if = urdf_joints[i]->type == urdf::Joint::CONTINUOUS ? "" : "non-";
-
- ROS_DEBUG_STREAM("CDarwinSim : Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << this->getHardwareInterfaceType() << "'.");
- }
-
- ROS_DEBUG_STREAM("CDarwinSim : Initialized controller '" << name_ << "' with:" <<
- "\n- Number of joints: " << joints_.size() <<
- "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'");
-
- /* initialize the RAM */
- ram_init();
- /* initialize the IMU */
- imu_init();
- /* initialize dynamixel slave */
- darwin_dyn_slave_init();
- // initialize dynamixel slave server
- ROS_INFO("Load dynamixel slave parameters");
- serial_device="/dev/ttyUSB0";
- controller_nh_.getParam("dyn_serial_device", serial_device);
- ROS_INFO_STREAM("dyn_serial_device is: " << serial_device);
- try{
- this->dyn_slave=new CDynamixelSlaveSerial("dyn_slave",serial_device,dyn_version2);
- this->dyn_slave->set_baudrate(2000000/(ram_data[DARWIN_BAUD_RATE]+1));
- this->dyn_slave->set_return_delay(ram_data[DARWIN_RETURN_DELAY_TIME]);
- this->dyn_slave->set_return_delay(ram_data[DARWIN_RETURN_LEVEL]);
- this->dyn_slave->add_slave(ram_data[DARWIN_ID],boost::bind(&darwin_on_ping),boost::bind(&darwin_on_read,_1,_2,_3),boost::bind(&darwin_on_write,_1,_2,_3));
- }catch(CException &e){
- if(this->dyn_slave!=NULL)
- {
- delete this->dyn_slave;
- this->dyn_slave=NULL;
- }
- ROS_ERROR_STREAM(e.what());
- }
- // initialize IMU simulator
- ROS_INFO("Load IMU simulator");
- serial_device="/dev/ttyUSB0";
- controller_nh_.getParam("imu_serial_device", serial_device);
- ROS_INFO_STREAM("dyn_serial_device is: " << serial_device);
- try{
- this->imu_sim=new CBNO055IMUSim("darwin_imu",serial_device);
- this->imu_sim->set_calibrated();
- }catch(CException &e){
- if(this->imu_sim!=NULL)
- {
- delete this->imu_sim;
- this->imu_sim=NULL;
- }
- ROS_ERROR_STREAM(e.what());
- }
- /* initialize motion modules */
- manager_init(7800);// motion manager period in us
- ROS_INFO("CDarwinSim : motion manager initialized");
- /* go to the walk ready position */
- for(unsigned int i=0;i<20;++i)
- {
- manager_set_module(i+1,MM_ACTION);
- manager_enable_servo(i+1);
- }
- for(unsigned int i=20;i<n_joints;++i)
- {
- manager_set_module(i+1,MM_JOINTS);
- manager_enable_servo(i+1);
- }
- manager_enable_balance();// enable balance
- action_load_page(WALK_READY);
- action_start_page();
-
- return true;
- }
-
- template <class HardwareInterface>
- void DarwinControllerCPP<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
- {
- static bool gyro_calibrated=false;
- static ros::Time last_time=ros::Time(0,0);
- ros::Time current_time=ros::Time::now();
- std::vector<double> target_angles(joints_.size());
-
- /* get the actual simulation angles */
- for(unsigned int i=0;i<this->joints_.size();++i)
- real_angles[i]=joints_[i].getPosition();
- __HAL_TIM_SET_FLAG(TIM5,TIM_FLAG_CC1);
- __HAL_TIM_SET_IT_SOURCE(TIM5,TIM_IT_CC1);
- TIM5_IRQHandler();
- for (unsigned int i = 0; i < this->joints_.size(); ++i)
- {
- target_angles[i] = ((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0;
- double command = pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
- this->joints_[i].setCommand(command);
- }
- }
-
-/****************************************** imu *************************************************/
- template <class HardwareInterface>
- void DarwinControllerCPP<HardwareInterface>::gyro_callback(const sensor_msgs::Imu::ConstPtr& msg)
- {
- double gyro_x,gyro_y;
-
- // saturate to +/- 500 dps;
- gyro_x=msg->angular_velocity.y*180.0/3.14159;
- if(gyro_x>500.0)
- gyro_x=500.0;
- else if(gyro_x<-500.0)
- gyro_x=-500.0;
- gyro_data[0]=-(int32_t)(gyro_x*1000.0);
- gyro_y=msg->angular_velocity.z*180.0/3.14159;
- if(gyro_y>500.0)
- gyro_y=500.0;
- else if(gyro_y<-500.0)
- gyro_y=-500.0;
- gyro_data[1]=-(int32_t)(gyro_y*1000.0);
- }
-
- template <class HardwareInterface>
- void DarwinControllerCPP<HardwareInterface>::accel_callback(const sensor_msgs::Imu::ConstPtr& msg)
- {
- accel_data[0]=(int32_t)msg->linear_acceleration.x*1000.0;
- accel_data[1]=(int32_t)msg->linear_acceleration.y*1000.0;
- accel_data[2]=(int32_t)msg->linear_acceleration.z*1000.0;
- }
-
- template <class HardwareInterface>
- void DarwinControllerCPP<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
- {
- if(this->imu_sim!=NULL)
- {
- this->imu_sim->set_quaternion_data(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
- tf::Quaternion q(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
- tf::Matrix3x3 m(q);
- double roll, pitch, yaw;
- m.getRPY(roll, pitch, yaw);
- this->imu_sim->set_euler_angles_data(yaw,roll,pitch);
- this->imu_sim->set_linear_accel_data(msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z);
- this->imu_sim->set_gyro_data(msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z);
- }
- }
-
-/****************************************** imu *************************************************/
-
- template <class HardwareInterface>
- DarwinControllerCPP<HardwareInterface>::~DarwinControllerCPP()
- {
- if(this->dyn_slave!=NULL)
- {
- delete this->dyn_slave;
- this->dyn_slave=NULL;
- }
- if(this->imu_sim!=NULL)
- {
- delete this->imu_sim;
- this->imu_sim=NULL;
- }
- }
-
-} // namespace
-
-#endif // header guard
diff --git a/include/darwin_controller_cpp.h b/include/dynamixel_robot_gazebo.h
similarity index 61%
rename from include/darwin_controller_cpp.h
rename to include/dynamixel_robot_gazebo.h
index 808e89e5c6f4dac55cfee9bb316788ebad515133..2334aa88312e9680326c304ae4e7829e2d55fa04 100644
--- a/include/darwin_controller_cpp.h
+++ b/include/dynamixel_robot_gazebo.h
@@ -28,18 +28,11 @@
/// \author Adolfo Rodriguez Tsouroukdissian, Stuart Glaser
-#ifndef DARWIN_CONTROLLER_CPP_H
-#define DARWIN_CONTROLLER_CPP_H
-
-// C++ standard
-#include <cassert>
-#include <iterator>
-#include <stdexcept>
-#include <string>
+#ifndef DYNAMIXEL_ROBOT_GAZEBO_H
+#define DYNAMIXEL_ROBOT_GAZEBO_H
// Boost
#include <boost/shared_ptr.hpp>
-#include <boost/scoped_ptr.hpp>
// ROS
#include <ros/node_handle.h>
@@ -50,63 +43,40 @@
// ros_controls
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
-#include <control_toolbox/pid.h>
-#include <sensor_msgs/Imu.h>
-#include <tf/transform_datatypes.h>
+#include <pluginlib/class_list_macros.h>
-#include "mutex.h"
#include "dynamixel_slave_serial.h"
-#include "bno055_imu_sim.h"
+#include "dynamixel_servo.h"
-namespace darwin_controller_cpp
+namespace dynamixel_robot_gazebo
{
/**
* \brief
*
*/
- template <class HardwareInterface>
- class DarwinControllerCPP : public controller_interface::Controller<HardwareInterface>
+ class DynamixelRobotGazebo : public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
public:
- DarwinControllerCPP();
- ~DarwinControllerCPP();
+ DynamixelRobotGazebo();
+ virtual ~DynamixelRobotGazebo();
- bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+ using controller_interface::Controller<hardware_interface::EffortJointInterface>::init;
+ bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
void starting(const ros::Time& time);
void stopping(const ros::Time& time);
void update(const ros::Time& time, const ros::Duration& period);
private:
- typedef typename HardwareInterface::ResourceHandleType JointHandle;
- typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
-
CDynamixelSlaveSerial *dyn_slave;
-
- CBNO055IMUSim *imu_sim;
-
- std::string name_; ///< Controller name.
- std::vector<JointHandle> joints_; ///< Handles to controlled joints.
- std::vector<std::string> joint_names_; ///< Controlled joint names.
- std::vector<PidPtr> pids_;
+ std::vector<CDynServo *> servos;
+ std::string name;
+ std::vector<std::string> joint_names;
// ROS API
- ros::NodeHandle controller_nh_;
-
- // topic subscribers
- ros::Subscriber gyro_sub;
- void gyro_callback(const sensor_msgs::Imu::ConstPtr& msg);
-
- ros::Subscriber accel_sub;
- void accel_callback(const sensor_msgs::Imu::ConstPtr& msg);
-
- ros::Subscriber imu_sub;
- void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
-
- CMutex walk_access;
+ ros::NodeHandle controller_nh;
};
} // namespace
-#include <darwin_controller_cpp_impl.h>
#endif // header guard
diff --git a/package.xml b/package.xml
index 9fce760b828283f5b17541e8aaf9b78e050e27ab..10386417993b303aa23538e794ad1685d5e1608c 100644
--- a/package.xml
+++ b/package.xml
@@ -1,31 +1,31 @@
<?xml version="1.0"?>
<package>
- <name>darwin_controller_cpp</name>
+ <name>dynamixel_robot_gazebo</name>
<version>0.0.0</version>
- <description>The darwin_controller_cpp package</description>
+ <description>The dynamixel_robot_gazebo 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>
+ <maintainer email="shernand@iri.upc.edu">Sergi Hernandez Juan</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>TODO</license>
+ <license>GPLv3</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/darwin_controller_cpp</url> -->
+ <!-- <url type="website">http://wiki.ros.org/dynamixel_robot_gazebo</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> -->
+ <author email="shernand@iri.upc.edu">Sergi Hernandez Juan</author>
<!-- The *_depend tags are used to specify dependencies -->
@@ -61,6 +61,6 @@
<run_depend>gazebo_ros</run_depend>
<export>
- <controller_interface plugin="${prefix}/ros_control_plugins.xml"/>
+ <controller_interface plugin="${prefix}/dynamixel_robot_plugin.xml"/>
</export>
</package>
diff --git a/ros_control_plugins.xml b/ros_control_plugins.xml
deleted file mode 100644
index bfaaac29381451b0c74236eea2f066702b47b1fe..0000000000000000000000000000000000000000
--- a/ros_control_plugins.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-<library path="lib/libdarwin_controller_cpp">
-
- <class name="effort_controllers/DarwinControllerCPP"
- type="effort_controllers::DarwinControllerCPP"
- base_class_type="controller_interface::ControllerBase">
- <description>
- this is a description
- </description>
- </class>
-
-</library>
-
diff --git a/src/darwin_controller_cpp.cpp b/src/darwin_controller_cpp.cpp
deleted file mode 100644
index f24c4ae4288c54a64c07ca3750f770ecb8dfb2bf..0000000000000000000000000000000000000000
--- a/src/darwin_controller_cpp.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-///////////////////////////////////////////////////////////////////////////////
-// Copyright (C) 2013, PAL Robotics S.L.
-// Copyright (c) 2008, Willow Garage, Inc.
-//
-// 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 PAL Robotics S.L. 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.
-//////////////////////////////////////////////////////////////////////////////
-
-// Pluginlib
-#include <pluginlib/class_list_macros.h>
-
-// Project
-#include <darwin_controller_cpp.h>
-
-namespace effort_controllers
-{
- /**
- * \brief Joint trajectory controller that represents trajectory segments as <b>quintic splines</b> and sends
- * commands to a \b position interface.
- */
- typedef darwin_controller_cpp::DarwinControllerCPP<hardware_interface::EffortJointInterface> DarwinControllerCPP;
-}
-
-PLUGINLIB_EXPORT_CLASS(effort_controllers::DarwinControllerCPP, controller_interface::ControllerBase)
diff --git a/src/dynamixel_robot_gazebo.cpp b/src/dynamixel_robot_gazebo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b3ce8163168fde1a495e51788736b6afa532b62e
--- /dev/null
+++ b/src/dynamixel_robot_gazebo.cpp
@@ -0,0 +1,237 @@
+#ifndef DYNAMIXEL_ROBOT_GAZEBO_IMP_H
+#define DYNAMIXEL_ROBOT_GAZEBO_IMP_H
+
+#include <sys/stat.h>
+
+#include "exceptions.h"
+#include "dynamixel_robot_gazebo.h"
+
+namespace dynamixel_robot_gazebo
+{
+ namespace internal
+ {
+ std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string& param_name)
+ {
+ using namespace XmlRpc;
+ XmlRpcValue xml_array;
+ if (!nh.getParam(param_name, xml_array))
+ {
+ ROS_ERROR_STREAM("Dynamixel robot gazebo: Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+ if (xml_array.getType() != XmlRpcValue::TypeArray)
+ {
+ ROS_ERROR_STREAM("Dynamixel robot gazebo: The '" << param_name << "' parameter is not an array (namespace: " <<
+ nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+
+ std::vector<std::string> out;
+ for (int i = 0; i < xml_array.size(); ++i)
+ {
+ if (xml_array[i].getType() != XmlRpcValue::TypeString)
+ {
+ ROS_ERROR_STREAM("Dynamixel robot gazebo: The '" << param_name << "' parameter contains a non-string element (namespace: " <<
+ nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+ out.push_back(static_cast<std::string>(xml_array[i]));
+ }
+ return out;
+ }
+
+ boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
+ {
+ boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
+
+ std::string urdf_str;
+ // Check for robot_description in proper namespace
+ if (nh.getParam(param_name, urdf_str))
+ {
+ if (!urdf->initString(urdf_str))
+ {
+ ROS_ERROR_STREAM("Dynamixel robot gazebo: Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
+ nh.getNamespace() << ").");
+ return boost::shared_ptr<urdf::Model>();
+ }
+ }
+ // Check for robot_description in root
+ else if (!urdf->initParam("robot_description"))
+ {
+ ROS_ERROR_STREAM("Dynamixel robot gazebo: Failed to parse URDF contained in '" << param_name << "' parameter");
+ return boost::shared_ptr<urdf::Model>();
+ }
+ return urdf;
+ }
+
+ typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
+ std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
+ {
+ std::vector<UrdfJointConstPtr> out;
+ for (unsigned int i = 0; i < joint_names.size(); ++i)
+ {
+ UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
+ if (urdf_joint)
+ {
+ out.push_back(urdf_joint);
+ }
+ else
+ {
+ ROS_ERROR_STREAM("Dynamixel robot gazebo: Could not find joint '" << joint_names[i] << "' in URDF model.");
+ return std::vector<UrdfJointConstPtr>();
+ }
+ }
+ return out;
+ }
+
+ std::string getLeafNamespace(const ros::NodeHandle& nh)
+ {
+ const std::string complete_ns = nh.getNamespace();
+ std::size_t id = complete_ns.find_last_of("/");
+ return complete_ns.substr(id + 1);
+ }
+
+ } // namespace
+
+ inline void DynamixelRobotGazebo::starting(const ros::Time& time)
+ {
+ }
+
+ inline void DynamixelRobotGazebo::stopping(const ros::Time& time)
+ {
+ }
+
+ DynamixelRobotGazebo::DynamixelRobotGazebo()
+ {
+ this->dyn_slave=NULL;
+ }
+
+ bool DynamixelRobotGazebo::init(hardware_interface::EffortJointInterface* hw,ros::NodeHandle& root_nh,ros::NodeHandle& controller_nh)
+ {
+ unsigned int n_joints;
+ std::string device_name;
+ int return_delay=0,return_level=2,protocol=2,baudrate=1000000;
+ struct stat buffer;
+
+ // Cache controller node handle
+ this->controller_nh = controller_nh;
+
+ // Controller name
+ this->name=internal::getLeafNamespace(this->controller_nh);
+
+ // List of controlled joints
+ this->joint_names=internal::getStrings(this->controller_nh,"joints");
+ if(this->joint_names.empty())
+ {
+ ROS_ERROR_STREAM("Dynamixel gazebo plugin: no controlled joints");
+ return false;
+ }
+ n_joints=this->joint_names.size();
+
+ // URDF joints
+ boost::shared_ptr<urdf::Model> urdf=internal::getUrdf(root_nh, "robot_description");
+ if(!urdf)
+ {
+ ROS_ERROR_STREAM("Dynamixel gazebo plugin: impossible to read the robot_description parameter");
+ return false;
+ }
+
+ std::vector<internal::UrdfJointConstPtr> urdf_joints=internal::getUrdfJoints(*urdf, this->joint_names);
+ if(n_joints!=urdf_joints.size())
+ {
+ ROS_ERROR_STREAM("Dynamixel gazebo plugin: some joints missing from the URDF model");
+ return false;
+ }
+
+ // initialize dynamixel slave server
+ ROS_INFO("Dynamixel gazebo plugin: Load dynamixel slave parameters");
+ ros::NodeHandle dynamixel_nh(this->controller_nh,std::string("dynamixel_bus"));
+ device_name="/tmp"+root_nh.getNamespace()+"_gazebo";
+ // wait until the serial device exists
+ unsigned int timeout=20;
+ do{
+ usleep(100000);
+ timeout--;
+ }while(stat(device_name.c_str(),&buffer)!=0 && timeout>0);
+ if(timeout==0)
+ {
+ ROS_ERROR_STREAM("Dynamixel gazebo plugin: serial device " << device_name << " not present");
+ return false;
+ }
+
+ try{
+ dynamixel_nh.getParam("protocol", protocol);
+ if(protocol==1)
+ this->dyn_slave=new CDynamixelSlaveSerial(root_nh.getNamespace()+"_slave",device_name,dyn_version1);
+ else if(protocol==2)
+ this->dyn_slave=new CDynamixelSlaveSerial(root_nh.getNamespace()+"_slave",device_name,dyn_version2);
+ else
+ {
+ ROS_ERROR_STREAM("Dynamixel gazebo plugin: Invalid protocol version");
+ return false;
+ }
+ dynamixel_nh.getParam("baudrate", baudrate);
+ this->dyn_slave->set_baudrate(baudrate);
+ dynamixel_nh.getParam("return_delay", return_delay);
+ this->dyn_slave->set_return_delay(return_delay);
+ dynamixel_nh.getParam("return_level", return_level);
+ if(return_level==0 || return_level==1 || return_level==2)
+ this->dyn_slave->set_return_delay(return_level);
+ else
+ {
+ ROS_ERROR_STREAM("Dynamixel gazebo plugin: Invalid return level");
+ delete this->dyn_slave;
+ this->dyn_slave=NULL;
+ return false;
+ }
+ }catch(CException &e){
+ if(this->dyn_slave!=NULL)
+ {
+ delete this->dyn_slave;
+ this->dyn_slave=NULL;
+ }
+ ROS_ERROR_STREAM("Dynamixel gazebo plugin: " << e.what());
+ }
+ // initialize joints
+ this->servos.resize(n_joints);
+ for(unsigned int i=0;i<n_joints;++i)
+ this->servos[i]=new CDynServo(this->joint_names[i],this->dyn_slave,hw,controller_nh);
+
+ ROS_DEBUG_STREAM("Dynamixel gazebo plugin: Initialized controller '" << this->name << "' with:" << "\n- Number of joints: " << this->servos.size() << "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'");
+
+ return true;
+ }
+
+ void DynamixelRobotGazebo::update(const ros::Time& time, const ros::Duration& period)
+ {
+ try{
+ /* get the actual simulation angles */
+ for(unsigned int i=0;i<this->servos.size();++i)
+ this->servos[i]->update(period);
+ }catch(CException &e){
+
+ }
+ }
+
+ DynamixelRobotGazebo::~DynamixelRobotGazebo()
+ {
+ try{
+ for(unsigned int i=0;i<this->servos.size();i++)
+ {
+ this->dyn_slave->delete_slave(this->servos[i]->get_id());
+ delete this->servos[i];
+ this->servos[i]=NULL;
+ }
+ if(this->dyn_slave!=NULL)
+ {
+ delete this->dyn_slave;
+ this->dyn_slave=NULL;
+ }
+ }catch(CException &e){
+ }
+ }
+} // namespace
+
+PLUGINLIB_EXPORT_CLASS(dynamixel_robot_gazebo::DynamixelRobotGazebo, controller_interface::ControllerBase)
+
+#endif // header guard