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

latest pushes

Browse Source
This commit is contained in:
Ronaldson Bellande
2023-10-03 21:27:53 -04:00
parent dbbadbe0ca
commit c7565da012
9 changed files with 280 additions and 330 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
humanoid_robot_controller/CMakeLists.txt
View File

@ -1,32 +1,31 @@
################################################################################
# Set minimum required version of cmake, project name and compile options
################################################################################
cmake_minimum_required(VERSION 3.8)
project(humanoid_robot_controller)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
################################################################################
# Find catkin packages and libraries for catkin and system dependencies
################################################################################
find_package(catkin REQUIRED COMPONENTS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_controller_msgs
dynamixel_sdk
humanoid_robot_device
humanoid_robot_framework_common
cmake_modules
)
find_package(Boost REQUIRED)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_controller_msgs
dynamixel_sdk
humanoid_robot_device
humanoid_robot_framework_common
cmake_modules
)
find_package(Boost REQUIRED)
find_package(PkgConfig REQUIRED)
else()
find_package(ament_cmake REQUIRED)
endif()
# Resolve system dependency on yaml-cpp, which apparently does not
# provide a CMake find_package() module.
find_package(PkgConfig REQUIRED)
pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
find_path(YAML_CPP_INCLUDE_DIR
NAMES yaml_cpp.h
@ -42,40 +41,28 @@ if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
add_definitions(-DHAVE_NEW_YAMLCPP)
endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
################################################################################
# Setup for python modules and scripts
################################################################################
################################################################################
# Declare ROS messages, services and actions
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_controller
CATKIN_DEPENDS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_controller_msgs
dynamixel_sdk
humanoid_robot_device
humanoid_robot_framework_common
cmake_modules
DEPENDS Boost
)
else()
ament_package()
endif()
################################################################################
# Declare ROS dynamic reconfigure parameters
################################################################################
################################################################################
# Declare catkin specific configuration to be passed to dependent projects
################################################################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_controller
CATKIN_DEPENDS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_controller_msgs
dynamixel_sdk
humanoid_robot_device
humanoid_robot_framework_common
cmake_modules
DEPENDS Boost
)
################################################################################
# Build
################################################################################
include_directories(
include
${catkin_INCLUDE_DIRS}
@ -87,9 +74,6 @@ add_library(humanoid_robot_controller src/humanoid_robot_controller/humanoid_rob
add_dependencies(humanoid_robot_controller ${catkin_EXPORTED_TARGETS})
target_link_libraries(humanoid_robot_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES})
################################################################################
# Install
################################################################################
install(TARGETS humanoid_robot_controller
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@ -99,7 +83,3 @@ install(TARGETS humanoid_robot_controller
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
################################################################################
# Test
################################################################################

146
humanoid_robot_controller/include/humanoid_robot_controller/humanoid_robot_controller.h
View File

@ -1,17 +1,17 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
@ -22,29 +22,29 @@
*/
#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
* humanoid_robot_controller.h
*
#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
* Author: zerom
*/
#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include "humanoid_robot_controller_msgs/GetJointModule.h"
#include "humanoid_robot_controller_msgs/JointCtrlModule.h"
#include "humanoid_robot_controller_msgs/LoadOffset.h"
#include "humanoid_robot_controller_msgs/SetJointModule.h"
#include "humanoid_robot_controller_msgs/SetModule.h"
#include "humanoid_robot_controller_msgs/SyncWriteItem.h"
#include "humanoid_robot_controller_msgs/WriteControlTable.h"
#include "dynamixel_sdk/group_bulk_read.h"
#include "dynamixel_sdk/group_sync_write.h"
#include "humanoid_robot_device/robot.h"
#include "humanoid_robot_framework_common/motion_module.h"
#include "humanoid_robot_framework_common/sensor_module.h"
namespace humanoid_robot_framework {
@ -74,7 +74,7 @@ private:
void gazeboTimerThread();
void msgQueueThread();
void setCtrlModuleThread(std::string ctrl_module);
#include "humanoid_robot_device/robot.h"
void setJointCtrlModuleThread(
const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg);
bool isTimerStopped();
@ -92,23 +92,23 @@ public:
std::map<std::string, dynamixel::GroupBulkRead *> port_to_bulk_read_;
/* sync write */
bool init_pose_loaded_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_;
bool is_offset_enabled_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_;
bool stop_timer_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_current_;
ControllerMode controller_mode_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_p_gain_;
std::list<MotionModule *> motion_modules_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_i_gain_;
std::vector<dynamixel::GroupSyncWrite *> direct_sync_write_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_d_gain_;
std::map<std::string, double> sensor_result_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_p_gain_;
void gazeboTimerThread();
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_i_gain_;
void setCtrlModuleThread(std::string ctrl_module);
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_d_gain_;
/* publisher */
@ -124,7 +124,7 @@ public:
RobotisController();
/* bulk read */
bool initialize(const std::string robot_file_path,
const std::string init_file_path);
void initializeDevice(const std::string init_file_path);
void process();
@ -142,66 +142,68 @@ public:
void loadOffset(const std::string path);
/* ROS Topic Callback Functions */
port_to_sync_write_position_i_gain_;
void writeControlTableCallback(
const humanoid_robot_controller_msgs::WriteControlTable::ConstPtr &msg);
void syncWriteItemCallback(
const humanoid_robot_controller_msgs::SyncWriteItem::ConstPtr &msg);
void setControllerModeCallback(const std_msgs::String::ConstPtr &msg);
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
port_to_sync_write_velocity_p_gain_;
void setJointCtrlModuleCallback(
const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg);
void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg);
void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg);
std::map<std::string, dynamixel::GroupSyncWrite *>
bool getJointCtrlModuleService(
humanoid_robot_controller_msgs::GetJointModule::Request &req,
humanoid_robot_controller_msgs::GetJointModule::Response &res);
/* publisher */
bool setJointCtrlModuleService(
humanoid_robot_controller_msgs::SetJointModule::Request &req,
humanoid_robot_controller_msgs::SetJointModule::Response &res);
ros::Publisher present_joint_state_pub_;
ros::Publisher current_module_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_position_pub_;
bool setCtrlModuleService(
humanoid_robot_controller_msgs::SetModule::Request &req,
humanoid_robot_controller_msgs::SetModule::Response &res);
bool
loadOffsetService(humanoid_robot_controller_msgs::LoadOffset::Request &req,
humanoid_robot_controller_msgs::LoadOffset::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);
int action(const std::string joint_name);
int reboot(const std::string joint_name, uint8_t *error = 0);
const std::string init_file_path);
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);
void removeMotionModule(MotionModule *module);
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);
void stopTimer();
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);
void writeControlTableCallback(
int writeCtrlItem(const std::string joint_name, const std::string item_name,
uint32_t data, uint8_t *error = 0);
void syncWriteItemCallback(
int write1Byte(const std::string joint_name, uint16_t address, uint8_t data,
uint8_t *error = 0);
const humanoid_robot_controller_msgs::SyncWriteItem::ConstPtr &msg);
int write2Byte(const std::string joint_name, uint16_t address, uint16_t data,
uint8_t *error = 0);
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
int write4Byte(const std::string joint_name, uint16_t address, uint32_t data,
uint8_t *error = 0);
const humanoid_robot_controller_msgs::JointCtrlModule::ConstPtr &msg);
int regWrite(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
};
humanoid_robot_controller_msgs::GetJointModule::Request &req,
} // namespace humanoid_robot_framework
#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */

58
humanoid_robot_device/CMakeLists.txt
View File

@ -1,43 +1,30 @@
################################################################################
# Set minimum required version of cmake, project name and compile options
################################################################################
cmake_minimum_required(VERSION 3.8)
project(humanoid_robot_device)
################################################################################
# Find catkin packages and libraries for catkin and system dependencies
################################################################################
find_package(catkin REQUIRED COMPONENTS
roscpp
dynamixel_sdk
)
################################################################################
# Setup for python modules and scripts
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
roscpp
dynamixel_sdk
)
else()
find_package(ament_cmake REQUIRED)
endif()
################################################################################
# Declare ROS messages, services and actions
################################################################################
################################################################################
# Declare ROS dynamic reconfigure parameters
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_device
CATKIN_DEPENDS
roscpp
dynamixel_sdk
)
else()
ament_package()
endif()
################################################################################
# Declare catkin specific configuration to be passed to dependent projects
################################################################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_device
CATKIN_DEPENDS
roscpp
dynamixel_sdk
)
################################################################################
# Build
################################################################################
include_directories(
include
${catkin_INCLUDE_DIRS}
@ -51,9 +38,6 @@ add_library(humanoid_robot_device
add_dependencies(humanoid_robot_device ${catkin_EXPORTED_TARGETS})
target_link_libraries(humanoid_robot_device ${catkin_LIBRARIES})
################################################################################
# Install
################################################################################
install(TARGETS humanoid_robot_device
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@ -67,7 +51,3 @@ install(DIRECTORY include/${PROJECT_NAME}/
install(DIRECTORY devices
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
################################################################################
# Test
################################################################################

42
humanoid_robot_device/include/humanoid_robot_device/control_table_item.h
View File

@ -1,17 +1,17 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
@ -22,8 +22,8 @@
*/
#ifndef ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_
* See the License for the specific language governing permissions and
#define ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_
#include <stdint.h>
namespace humanoid_robot_framework {
@ -43,12 +43,12 @@ public:
int32_t data_max_value_;
bool is_signed_;
enum AccessType { Read, ReadWrite };
ControlTableItem()
: item_name_(""), address_(0), access_type_(Read), memory_type_(RAM),
data_length_(0), data_min_value_(0), data_max_value_(0),
is_signed_(false) {}
};
public:
std::string item_name_;
} // namespace humanoid_robot_framework
#endif /* ROBOTIS_DEVICE_CONTROL_TABLE_ITEM_H_ */

40
humanoid_robot_device/include/humanoid_robot_device/time_stamp.h
View File

@ -1,18 +1,18 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
* time_stamp.h
@ -24,22 +24,16 @@
#ifndef ROBOTIS_DEVICE_TIME_STAMP_H_
#define ROBOTIS_DEVICE_TIME_STAMP_H_
namespace humanoid_robot_framework
{
namespace humanoid_robot_framework {
class TimeStamp {
public:
long sec_;
long nsec_;
TimeStamp(long sec, long nsec)
: sec_(sec),
nsec_(nsec)
{ }
TimeStamp(long sec, long nsec) : sec_(sec), nsec_(nsec) {}
};
}
} // namespace humanoid_robot_framework
#endif /* ROBOTIS_DEVICE_TIME_STAMP_H_ */

108
humanoid_robot_device/src/humanoid_robot_device/robot.cpp
View File

@ -1,17 +1,17 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
@ -21,30 +21,30 @@
* Author: zerom
*/
* See the License for the specific language governing permissions and
#include <algorithm>
#include <fstream>
#include <iostream>
#include "humanoid_robot_device/robot.h"
using namespace humanoid_robot_framework;
static inline std::string &ltrim(std::string &s) {
*
* Created on: 2015. 12. 11.
s.erase(s.begin(),
std::find_if(s.begin(), s.end(),
std::not1(std::ptr_fun<int, int>(std::isspace))));
return s;
}
static inline std::string &rtrim(std::string &s) {
#include <fstream>
s.erase(std::find_if(s.rbegin(), s.rend(),
std::not1(std::ptr_fun<int, int>(std::isspace)))
.base(),
s.end());
return s;
}
static inline std::string &trim(std::string &s) { return ltrim(rtrim(s)); }
using namespace humanoid_robot_framework;
static inline std::vector<std::string> split(const std::string &text,
char sep) {
std::vector<std::string> tokens;
std::size_t start = 0, end = 0;
@ -60,7 +60,7 @@ static inline std::vector<std::string> split(const std::string &text,
return tokens;
}
Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
: control_cycle_msec_(DEFAULT_CONTROL_CYCLE) {
if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0)
dev_desc_dir_path += "/";
@ -81,10 +81,10 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
input_str = trim(input_str);
// find session
}
if (!input_str.compare(0, 1, "[") &&
!input_str.compare(input_str.size() - 1, 1, "]")) {
input_str = input_str.substr(1, input_str.size() - 2);
std::transform(input_str.begin(), input_str.end(), input_str.begin(),
::tolower);
session = trim(input_str);
continue;
@ -102,10 +102,10 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
if (tokens.size() != 3)
continue;
input_str = trim(input_str);
std::cout << tokens[0] << " added. (baudrate: " << tokens[1] << ")"
<< std::endl;
if (!input_str.compare(0, 1, "[") &&
ports_[tokens[0]] =
dynamixel::PortHandler::getPortHandler(tokens[0].c_str());
ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str()));
port_default_device_[tokens[0]] = tokens[2];
@ -115,7 +115,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
continue;
if (tokens[0] == DYNAMIXEL) {
std::string file_path =
dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int id = std::atoi(tokens[2].c_str());
std::string port = tokens[1];
@ -127,20 +127,20 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
Dynamixel *dxl = dxls_[dev_name];
std::vector<std::string> sub_tokens = split(tokens[6], ',');
if (sub_tokens.size() > 0 && sub_tokens[0] != "") {
std::map<std::string, ControlTableItem *>::iterator indirect_it =
dxl->ctrl_table_.find(INDIRECT_ADDRESS_1);
if (indirect_it !=
dxl->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist
{
ports_[tokens[0]] =
uint16_t indirect_data_addr =
dxl->ctrl_table_[INDIRECT_DATA_1]->address_;
for (int _i = 0; _i < sub_tokens.size(); _i++) {
ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str()));
std::map<std::string, ControlTableItem *>::iterator
bulkread_it = dxl->ctrl_table_.find(sub_tokens[_i]);
if (bulkread_it == dxl->ctrl_table_.end()) {
std::vector<std::string> tokens = split(input_str, '|');
if (tokens.size() != 7)
continue;
fprintf(
stderr,
"\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n",
sub_tokens[_i].c_str());
continue;
}
@ -164,13 +164,13 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
{
for (int i = 0; i < sub_tokens.size(); i++) {
if (dxl->ctrl_table_[sub_tokens[i]] != NULL)
dxl->bulk_read_items_.push_back(
dxl->ctrl_table_[sub_tokens[i]]);
}
}
}
} else if (tokens[0] == SENSOR) {
if (bulkread_it == dxl->ctrl_table_.end()) {
std::string file_path =
dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int id = std::atoi(tokens[2].c_str());
std::string port = tokens[1];
@ -182,12 +182,12 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
Sensor *sensor = sensors_[dev_name];
std::vector<std::string> sub_tokens = split(tokens[6], ',');
if (sub_tokens.size() > 0 && sub_tokens[0] != "") {
ControlTableItem *dest_item = dxl->bulk_read_items_[_i];
std::map<std::string, ControlTableItem *>::iterator indirect_it =
sensor->ctrl_table_.find(INDIRECT_ADDRESS_1);
if (indirect_it !=
sensor->ctrl_table_.end()) // INDIRECT_ADDRESS_1 exist
{
dest_item->access_type_ = src_item->access_type_;
uint16_t indirect_data_addr =
sensor->ctrl_table_[INDIRECT_DATA_1]->address_;
for (int i = 0; i < sub_tokens.size(); i++) {
sensor->bulk_read_items_.push_back(new ControlTableItem());
@ -208,7 +208,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
} else // INDIRECT_ADDRESS_1 exist
{
for (int i = 0; i < sub_tokens.size(); i++)
sensor->bulk_read_items_.push_back(
sensor->ctrl_table_[sub_tokens[i]]);
}
}
@ -221,7 +221,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
}
}
Sensor *Robot::getSensor(std::string path, int id, std::string port,
float protocol_version) {
Sensor *sensor;
@ -245,10 +245,10 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port,
continue;
// find _session
}
if (!input_str.compare(0, 1, "[") &&
!input_str.compare(input_str.size() - 1, 1, "]")) {
input_str = input_str.substr(1, input_str.size() - 2);
for (int i = 0; i < sub_tokens.size(); i++)
std::transform(input_str.begin(), input_str.end(), input_str.begin(),
::tolower);
session = trim(input_str);
continue;
@ -293,9 +293,9 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port,
}
sensor->port_name_ = port;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->id_,
sensor->model_name_.c_str());
session = trim(input_str);
// std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name
// << " added. (" << port << ")" << std::endl;
file.close();
} else
@ -304,7 +304,7 @@ Sensor *Robot::getSensor(std::string path, int id, std::string port,
return sensor;
}
if (tokens[0] == "model_name")
Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port,
float protocol_version) {
Dynamixel *dxl;
@ -342,10 +342,10 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port,
continue;
// find session
if (!input_str.compare(0, 1, "[") &&
!input_str.compare(input_str.size() - 1, 1, "]")) {
input_str = input_str.substr(1, input_str.size() - 2);
} else
std::transform(input_str.begin(), input_str.end(), input_str.begin(),
::tolower);
session = trim(input_str);
continue;
@ -440,10 +440,10 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port,
if (dxl->ctrl_table_[torque_enable_item_name] != NULL)
dxl->torque_enable_item_ = dxl->ctrl_table_[torque_enable_item_name];
if (dxl->ctrl_table_[present_position_item_name] != NULL)
goal_velocity_item_name = tokens[1];
dxl->present_position_item_ =
dxl->ctrl_table_[present_position_item_name];
if (dxl->ctrl_table_[present_velocity_item_name] != NULL)
else if (tokens[0] == "position_d_gain_item_name")
dxl->present_velocity_item_ =
dxl->ctrl_table_[present_velocity_item_name];
if (dxl->ctrl_table_[present_current_item_name] != NULL)
dxl->present_current_item_ = dxl->ctrl_table_[present_current_item_name];
@ -466,9 +466,9 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port,
if (dxl->ctrl_table_[velocity_p_gain_item_name] != NULL)
dxl->velocity_p_gain_item_ = dxl->ctrl_table_[velocity_p_gain_item_name];
item->access_type_ = Read;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id_,
dxl->model_name_.c_str());
item->access_type_ = ReadWrite;
// std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << "
// added. (" << port << ")" << std::endl;
file.close();
} else

48
humanoid_robot_device/src/humanoid_robot_device/sensor.cpp
View File

@ -1,17 +1,17 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/*
@ -25,15 +25,15 @@
using namespace humanoid_robot_framework;
*******************************************************************************/
/*
* sensor.cpp
*
* Created on: 2016. 5. 11.
* Author: zerom
*/
Sensor::Sensor(int id, std::string model_name, float protocol_version) {
this->id_ = id;
this->model_name_ = model_name;
this->port_name_ = "";
this->protocol_version_ = protocol_version;
ctrl_table_.clear();
sensor_state_ = new SensorState();
bulk_read_items_.clear();
}

14
humanoid_robot_framework/CMakeLists.txt
View File

@ -1,4 +1,16 @@
cmake_minimum_required(VERSION 3.8)
project(humanoid_robot_framework)
find_package(catkin REQUIRED)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED)
else()
find_package(ament_cmake REQUIRED)
endif()
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package()
else()
ament_package()
endif()
catkin_metapackage()

56
humanoid_robot_framework_common/CMakeLists.txt
View File

@ -1,41 +1,30 @@
################################################################################
# Set minimum required version of cmake, project name and compile options
################################################################################
cmake_minimum_required(VERSION 3.8)
project(humanoid_robot_framework_common)
################################################################################
# Find catkin packages and libraries for catkin and system dependencies
################################################################################
find_package(catkin REQUIRED COMPONENTS
roscpp
humanoid_robot_device
)
################################################################################
# Setup for python modules and scripts
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
roscpp
humanoid_robot_device
)
else()
find_package(ament_cmake REQUIRED)
endif()
################################################################################
# Declare ROS messages, services and actions
################################################################################
################################################################################
# Declare ROS dynamic reconfigure parameters
################################################################################
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS
roscpp
humanoid_robot_device
)
else()
ament_package()
endif()
################################################################################
# Declare catkin specific configuration to be passed to dependent projects
################################################################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS roscpp humanoid_robot_device
)
################################################################################
# Build
################################################################################
include_directories(
include
${catkin_INCLUDE_DIRS}
@ -48,9 +37,6 @@ add_library(${PROJECT_NAME}
)
set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX)
################################################################################
# Install
################################################################################
install(TARGETS humanoid_robot_framework_common
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@ -60,7 +46,3 @@ install(TARGETS humanoid_robot_framework_common
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
################################################################################
# Test
################################################################################