- renewal
This commit is contained in:
ROBOTIS
55
robotis_controller/CMakeLists.txt
Normal file
55
robotis_controller/CMakeLists.txt
Normal file
@@ -0,0 +1,55 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(robotis_controller)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
roslib
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
robotis_device
|
||||
robotis_controller_msgs
|
||||
robotis_framework_common
|
||||
cmake_modules
|
||||
dynamixel_sdk
|
||||
)
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if you package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
# LIBRARIES robotis_controller
|
||||
CATKIN_DEPENDS roscpp roslib sensor_msgs std_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
# include_directories(include)
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a cpp library
|
||||
add_library(robotis_controller
|
||||
src/robotis_controller/RobotisController.cpp
|
||||
)
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
target_link_libraries(robotis_controller
|
||||
yaml-cpp
|
||||
robotis_device
|
||||
dynamixel_sdk
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
@@ -0,0 +1,124 @@
|
||||
/*
|
||||
* RobotisController.h
|
||||
*
|
||||
* Created on: 2016. 1. 15.
|
||||
* Author: zerom
|
||||
*/
|
||||
|
||||
#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_
|
||||
#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_
|
||||
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
|
||||
#include "robotis_device/Robot.h"
|
||||
#include "robotis_framework_common/MotionModule.h"
|
||||
|
||||
#include "robotis_controller_msgs/SyncWriteItem.h"
|
||||
#include "robotis_controller_msgs/JointCtrlModule.h"
|
||||
#include "robotis_controller_msgs/GetJointModule.h"
|
||||
|
||||
// TODO: TEMPORARY CODE !!
|
||||
#include "dynamixel_sdk/GroupBulkRead.h"
|
||||
#include "dynamixel_sdk/GroupSyncWrite.h"
|
||||
|
||||
namespace ROBOTIS
|
||||
{
|
||||
|
||||
enum CONTROLLER_MODE
|
||||
{
|
||||
MOTION_MODULE_MODE,
|
||||
DIRECT_CONTROL_MODE
|
||||
};
|
||||
|
||||
class RobotisController
|
||||
{
|
||||
private:
|
||||
static RobotisController *unique_instance_;
|
||||
|
||||
boost::thread queue_thread_;
|
||||
boost::mutex queue_mutex_;
|
||||
|
||||
bool is_timer_running_;
|
||||
bool stop_timer_;
|
||||
pthread_t timer_thread_;
|
||||
CONTROLLER_MODE controller_mode_;
|
||||
|
||||
std::list<MotionModule *> modules_;
|
||||
std::vector<GroupSyncWrite *> direct_sync_write_;
|
||||
|
||||
RobotisController();
|
||||
|
||||
void QueueThread();
|
||||
|
||||
bool CheckTimerStop();
|
||||
void InitSyncWrite();
|
||||
|
||||
public:
|
||||
static const int CONTROL_CYCLE_MSEC = 8; // 8 msec
|
||||
|
||||
bool DEBUG_PRINT;
|
||||
Robot *robot;
|
||||
|
||||
// TODO: TEMPORARY CODE !!
|
||||
std::map<std::string, GroupBulkRead *> port_to_bulk_read;
|
||||
std::map<std::string, GroupSyncWrite *> port_to_sync_write;
|
||||
|
||||
/* publisher */
|
||||
ros::Publisher goal_joint_state_pub;
|
||||
ros::Publisher present_joint_state_pub;
|
||||
ros::Publisher current_module_pub;
|
||||
|
||||
static void *ThreadProc(void *param);
|
||||
static RobotisController *GetInstance() { return unique_instance_; }
|
||||
|
||||
bool Initialize(const std::string robot_file_path, const std::string init_file_path);
|
||||
void Process();
|
||||
void AddModule(MotionModule *module);
|
||||
void RemoveModule(MotionModule *module);
|
||||
|
||||
void StartTimer();
|
||||
void StopTimer();
|
||||
bool IsTimerRunning();
|
||||
|
||||
void LoadOffset(const std::string path);
|
||||
|
||||
/* ROS Topic Callback Functions */
|
||||
void SyncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg);
|
||||
void SetControllerModeCallback(const std_msgs::String::ConstPtr &msg);
|
||||
void SetJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
|
||||
void SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
|
||||
bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res);
|
||||
|
||||
int Ping (const std::string joint_name, UINT8_T *error = 0);
|
||||
int Ping (const std::string joint_name, UINT16_T* model_number, UINT8_T *error = 0);
|
||||
|
||||
int Action (const std::string joint_name);
|
||||
int Reboot (const std::string joint_name, UINT8_T *error = 0);
|
||||
int FactoryReset(const std::string joint_name, UINT8_T option = 0, UINT8_T *error = 0);
|
||||
|
||||
int Read (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
|
||||
int ReadCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T *data, UINT8_T *error = 0);
|
||||
|
||||
int Read1Byte (const std::string joint_name, UINT16_T address, UINT8_T *data, UINT8_T *error = 0);
|
||||
int Read2Byte (const std::string joint_name, UINT16_T address, UINT16_T *data, UINT8_T *error = 0);
|
||||
int Read4Byte (const std::string joint_name, UINT16_T address, UINT32_T *data, UINT8_T *error = 0);
|
||||
|
||||
int Write (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
|
||||
int WriteCtrlItem(const std::string joint_name, const std::string item_name, UINT32_T data, UINT8_T *error = 0);
|
||||
|
||||
int Write1Byte (const std::string joint_name, UINT16_T address, UINT8_T data, UINT8_T *error = 0);
|
||||
int Write2Byte (const std::string joint_name, UINT16_T address, UINT16_T data, UINT8_T *error = 0);
|
||||
int Write4Byte (const std::string joint_name, UINT16_T address, UINT32_T data, UINT8_T *error = 0);
|
||||
|
||||
int RegWrite (const std::string joint_name, UINT16_T address, UINT16_T length, UINT8_T *data, UINT8_T *error = 0);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_ROBOTIS_CONTROLLER_ROBOTISCONTROLLER_H_ */
|
||||
38
robotis_controller/package.xml
Normal file
38
robotis_controller/package.xml
Normal file
@@ -0,0 +1,38 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>robotis_controller</name>
|
||||
<version>0.1.1</version>
|
||||
<description>The robotis_controller package</description>
|
||||
|
||||
<maintainer email="zerom@robotis.com">ROBOTIS</maintainer>
|
||||
|
||||
|
||||
<license>GPLv2</license>
|
||||
|
||||
|
||||
<!-- <url type="website">http://wiki.ros.org/robotis_controller</url> -->
|
||||
|
||||
|
||||
<author email="zerom@robotis.com">ROBOTIS</author>
|
||||
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>roslib</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>dynamixel_sdk</build_depend>
|
||||
<build_depend>robotis_device</build_depend>
|
||||
<build_depend>robotis_controller_msgs</build_depend>
|
||||
<build_depend>robotis_framework_common</build_depend>
|
||||
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>roslib</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>dynamixel_sdk</run_depend>
|
||||
<run_depend>robotis_device</run_depend>
|
||||
<run_depend>robotis_controller_msgs</run_depend>
|
||||
|
||||
</package>
|
||||
1112
robotis_controller/src/robotis_controller/RobotisController.cpp
Normal file
1112
robotis_controller/src/robotis_controller/RobotisController.cpp
Normal file
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user