BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- added code to support the gazebo simulator

Browse Source
This commit is contained in:
ROBOTIS
2016-03-10 19:57:33 +09:00
parent 33462ebfb7
commit 1648993922
3 changed files with 738 additions and 633 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
robotis_controller/include/robotis_controller/RobotisController.h
View File

@@ -13,6 +13,7 @@
#include <boost/thread.hpp>
#include <yaml-cpp/yaml.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>
#include "robotis_device/Robot.h"
@@ -41,8 +42,10 @@ private:
static RobotisController *unique_instance_;
boost::thread queue_thread_;
boost::mutex queue_mutex_;
boost::thread gazebo_thread_;
boost::mutex queue_mutex_;
bool init_pose_loaded_;
bool is_timer_running_;
bool stop_timer_;
pthread_t timer_thread_;
@@ -54,6 +57,7 @@ private:
RobotisController();
void QueueThread();
void GazeboThread();
bool CheckTimerStop();
void InitSyncWrite();
@@ -64,6 +68,9 @@ public:
bool DEBUG_PRINT;
Robot *robot;
bool gazebo_mode;
std::string gazebo_robot_name;
// TODO: TEMPORARY CODE !!
std::map<std::string, GroupBulkRead *> port_to_bulk_read;
std::map<std::string, GroupSyncWrite *> port_to_sync_write;
@@ -73,6 +80,8 @@ public:
ros::Publisher present_joint_state_pub;
ros::Publisher current_module_pub;
std::map<std::string, ros::Publisher> gazebo_joint_pub;
static void *ThreadProc(void *param);
static RobotisController *GetInstance() { return unique_instance_; }
@@ -94,6 +103,8 @@ public:
void SetCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
bool GetCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res);
void GazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
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);

1354
robotis_controller/src/robotis_controller/RobotisController.cpp
View File

File diff suppressed because it is too large Load Diff

4
robotis_device/src/robotis_device/Robot.cpp
View File

@@ -77,9 +77,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")" << std::endl;
ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str());
bool _port_result = ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str()));
if(_port_result == false)
exit(-1);
ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str()));
}
else if(session == "device info")
{