32 : is_timer_running_(false),
34 init_pose_loaded_(false),
40 gazebo_robot_name_(
"robotis")
52 it.second->txRxPacket();
53 for(
auto& it : port_to_bulk_read_)
59 if (++error_count > 10)
61 ROS_ERROR(
"[RobotisController] first bulk read fail!!");
65 result = it.second->txRxPacket();
74 if (it.second != NULL)
75 it.second->clearParam();
79 if (it.second != NULL)
80 it.second->clearParam();
84 if (it.second != NULL)
85 it.second->clearParam();
89 if (it.second != NULL)
90 it.second->clearParam();
94 if (it.second != NULL)
95 it.second->clearParam();
99 if (it.second != NULL)
100 it.second->clearParam();
104 if (it.second != NULL)
105 it.second->clearParam();
109 if (it.second != NULL)
110 it.second->clearParam();
114 if (it.second != NULL)
115 it.second->clearParam();
121 std::string joint_name = it.first;
126 uint32_t read_data = 0;
127 uint8_t sync_write_data[4];
129 if (port_to_bulk_read_[dxl->
port_name_]->isAvailable(dxl->
id_,
133 read_data = port_to_bulk_read_[dxl->
port_name_]->getData(dxl->
id_,
148 port_to_sync_write_position_[dxl->
port_name_]->addParam(dxl->
id_, sync_write_data);
202 robot_ =
new Robot(robot_file_path, dev_desc_dir_path);
212 std::string port_name = it.first;
224 auto dxl_it =
robot_->
dxls_.find(default_device_name);
228 Dynamixel *default_device = dxl_it->second;
314 Sensor *_default_device = sensor_it->second;
324 std::string joint_name = it.first;
327 if (
ping(joint_name) != 0)
330 if (
ping(joint_name) != 0)
331 ROS_ERROR(
"JOINT[%s] does NOT respond!!", joint_name.c_str());
350 doc = YAML::LoadFile(init_file_path.c_str());
352 for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++)
354 std::string joint_name = it_doc->first.as<std::string>();
356 YAML::Node joint_node = doc[joint_name];
357 if (joint_node.size() == 0)
363 dxl = dxl_it->second;
367 ROS_WARN(
"Joint [%s] was not found.", joint_name.c_str());
371 ROS_INFO(
"JOINT_NAME: %s", joint_name.c_str());
373 for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++)
375 std::string item_name = it_joint->first.as<std::string>();
378 ROS_INFO(
" ITEM_NAME: %s", item_name.c_str());
380 uint32_t value = it_joint->second.as<uint32_t>();
385 ROS_WARN(
"Control Item [%s] was not found.", item_name.c_str());
389 if (item->memory_type_ ==
EEPROM)
395 switch (item->data_length_)
398 read1Byte(joint_name, item->address_, &data8);
403 read2Byte(joint_name, item->address_, &data16);
408 read4Byte(joint_name, item->address_, &data32);
417 switch (item->data_length_)
420 write1Byte(joint_name, item->address_, (uint8_t) value);
423 write2Byte(joint_name, item->address_, (uint16_t) value);
426 write4Byte(joint_name, item->address_, value);
432 if (item->memory_type_ ==
EEPROM)
435 usleep(item->data_length_ * 55 * 1000);
439 }
catch (
const std::exception& e)
441 ROS_INFO(
"Dynamixel Init file not found.");
452 std::string joint_name = it.first;
458 int bulkread_start_addr = 0;
459 int bulkread_data_length = 0;
477 bulkread_data_length = 0;
480 int indirect_addr = indirect_addr_it->second->address_;
485 bulkread_data_length += addr_leng;
486 for (
int l = 0; l < addr_leng; l++)
490 read2Byte(joint_name, indirect_addr, &data16);
505 bulkread_data_length = 0;
512 if (addr < bulkread_start_addr)
513 bulkread_start_addr = addr;
514 else if (last_item->
address_ < addr)
523 if (bulkread_start_addr != 0)
533 std::string sensor_name = it.first;
534 Sensor *sensor = it.second;
539 int bulkread_start_addr = 0;
540 int bulkread_data_length = 0;
551 bulkread_data_length = 0;
554 int indirect_addr = indirect_addr_it->second->address_;
559 bulkread_data_length += addr_leng;
560 for (
int l = 0; l < addr_leng; l++)
563 read2Byte(sensor_name, indirect_addr, &data16);
580 bulkread_data_length = 0;
587 if (addr < bulkread_start_addr)
588 bulkread_start_addr = addr;
589 else if (last_item->
address_ < addr)
598 if (bulkread_start_addr != 0)
645 "/robotis/present_joint_ctrl_modules", 10);
676 static struct timespec next_time;
677 static struct timespec curr_time;
679 ROS_DEBUG(
"controller::thread_proc started");
681 clock_gettime(CLOCK_MONOTONIC, &next_time);
685 next_time.tv_sec += (next_time.tv_nsec + controller->
robot_->
getControlCycle() * 1000000) / 1000000000;
686 next_time.tv_nsec = (next_time.tv_nsec + controller->
robot_->
getControlCycle() * 1000000) % 1000000000;
690 clock_gettime(CLOCK_MONOTONIC, &curr_time);
691 long delta_nsec = (next_time.tv_sec - curr_time.tv_sec) * 1000000000 + (next_time.tv_nsec - curr_time.tv_nsec);
692 if (delta_nsec < -100000)
696 fprintf(stderr,
"[RobotisController::ThreadProc] NEXT TIME < CURR TIME.. (%f)[%ld.%09ld / %ld.%09ld]",
697 delta_nsec / 1000000.0, (
long )next_time.tv_sec, (
long )next_time.tv_nsec,
698 (
long )curr_time.tv_sec, (
long )curr_time.tv_nsec);
702 next_time.tv_sec = curr_time.tv_sec + (curr_time.tv_nsec + 3000000) / 1000000000;
703 next_time.tv_nsec = (curr_time.tv_nsec + 3000000) % 1000000000;
706 clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL);
727 it.second->txPacket();
733 struct sched_param param;
736 pthread_attr_init(&attr);
738 error = pthread_attr_setschedpolicy(&attr, SCHED_RR);
740 ROS_ERROR(
"pthread_attr_setschedpolicy error = %d\n", error);
741 error = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
743 ROS_ERROR(
"pthread_attr_setinheritsched error = %d\n", error);
745 memset(¶m, 0,
sizeof(param));
746 param.sched_priority = 31;
747 error = pthread_attr_setschedparam(&attr, ¶m);
749 ROS_ERROR(
"pthread_attr_setschedparam error = %d\n", error);
754 ROS_ERROR(
"Creating timer thread failed!!");
779 if (it.second != NULL)
780 it.second->rxPacket();
785 if (it.second != NULL)
786 it.second->clearParam();
790 if (it.second != NULL)
791 it.second->clearParam();
795 if (it.second != NULL)
796 it.second->clearParam();
800 if (it.second != NULL)
801 it.second->clearParam();
805 if (it.second != NULL)
806 it.second->clearParam();
810 if (it.second != NULL)
811 it.second->clearParam();
815 if (it.second != NULL)
816 it.second->clearParam();
820 if (it.second != NULL)
821 it.second->clearParam();
825 if (it.second != NULL)
826 it.second->clearParam();
850 doc = YAML::LoadFile(path.c_str());
851 }
catch (
const std::exception& e)
857 YAML::Node offset_node = doc[
"offset"];
858 if (offset_node.size() == 0)
862 for (YAML::const_iterator it = offset_node.begin(); it != offset_node.end(); it++)
864 std::string joint_name = it->first.as<std::string>();
865 double offset = it->second.as<
double>();
869 dxl_it->second->dxl_state_->position_offset_ = offset;
876 static bool is_process_running =
false;
877 if (is_process_running ==
true)
879 is_process_running =
true;
889 sensor_msgs::JointState goal_state;
890 sensor_msgs::JointState present_state;
893 goal_state.header.stamp = present_state.header.stamp;
903 it.second->rxPacket();
912 std::string port_name = dxl_it.second->
port_name_;
913 std::string joint_name = dxl_it.first;
917 bool updated =
false;
957 Sensor *sensor = sensor_it.second;
958 std::string port_name = sensor_it.second->
port_name_;
959 std::string sensor_name = sensor_it.first;
963 bool updated =
false;
988 fprintf(stderr,
"(%2.6f) BulkRead Rx & update state \n", time_duration.
nsec * 0.000001);
1008 it.second->txPacket();
1009 it.second->clearParam();
1016 it.second->txPacket();
1017 it.second->clearParam();
1024 it.second->txPacket();
1025 it.second->clearParam();
1032 it.second->txPacket();
1033 it.second->clearParam();
1040 it.second->txPacket();
1041 it.second->clearParam();
1048 it.second->txPacket();
1049 it.second->clearParam();
1054 if (it.second != NULL)
1055 it.second->txPacket();
1059 if (it.second != NULL)
1060 it.second->txPacket();
1064 if (it.second != NULL)
1065 it.second->txPacket();
1071 for (
auto& it : port_to_bulk_read_)
1072 it.second->txPacket();
1077 fprintf(stderr,
"(%2.6f) SyncWrite & BulkRead Tx \n", time_duration.
nsec * 0.000001);
1084 if ((*module_it)->getModuleEnable() ==
false)
1087 std_msgs::Float64 joint_msg;
1091 std::string joint_name = dxl_it.first;
1125 it.second->rxPacket();
1134 std::string port_name = dxl_it.second->
port_name_;
1135 std::string joint_name = dxl_it.first;
1192 for (
auto& it : port_to_bulk_read_)
1193 it.second->txPacket();
1205 for (
auto& it : (*module_it)->result_)
1213 fprintf(stderr,
"(%2.6f) SensorModule Process() & save result \n", time_duration.
nsec * 0.000001);
1226 if ((*module_it)->getModuleEnable() ==
false)
1234 std::string joint_name = dxl_it.first;
1241 DynamixelState *result_state = (*module_it)->result_[joint_name];
1243 if (result_state == NULL)
1245 ROS_ERROR(
"[%s] %s ", (*module_it)->getModuleName().c_str(), joint_name.c_str());
1259 uint8_t sync_write_data[4] = { 0 };
1272 uint8_t sync_write_data[4] = { 0 };
1286 uint8_t sync_write_data[4] = { 0 };
1300 uint8_t sync_write_data[4] = { 0 };
1318 uint8_t sync_write_data[4] = { 0 };
1331 uint8_t sync_write_data[4] = { 0 };
1345 uint8_t sync_write_data[4] = { 0 };
1359 uint8_t sync_write_data[4] = { 0 };
1377 uint8_t sync_write_data[2] = { 0 };
1395 fprintf(stderr,
"(%2.6f) MotionModule Process() & save result \n", time_duration.
nsec * 0.000001);
1402 std::string joint_name = dxl_it.first;
1405 present_state.name.push_back(joint_name);
1410 goal_state.name.push_back(joint_name);
1423 fprintf(stderr,
"(%2.6f) Process() DONE \n", time_duration.
nsec * 0.000001);
1426 is_process_running =
false;
1478 fprintf(stderr,
"[WriteControlTable] led control msg received\n");
1480 auto dev_it1 =
robot_->
dxls_.find(msg->joint_name);
1483 device = dev_it1->second;
1490 device = dev_it2->second;
1494 ROS_WARN(
"[WriteControlTable] Unknown device : %s", msg->joint_name.c_str());
1500 auto item_it = device->
ctrl_table_.find(msg->start_item_name);
1503 item = item_it->second;
1507 ROS_WARN(
"[WriteControlTable] Unknown item : %s", msg->start_item_name.c_str());
1533 for (
int i = 0; i < msg->joint_name.size(); i++)
1537 auto d_it1 =
robot_->
dxls_.find(msg->joint_name[i]);
1540 device = d_it1->second;
1547 device = d_it2->second;
1551 ROS_WARN(
"[SyncWriteItem] Unknown device : %s", msg->joint_name[i].c_str());
1558 auto item_it = device->
ctrl_table_.find(msg->item_name);
1561 item = item_it->second;
1565 ROS_WARN(
"SyncWriteItem] Unknown item : %s", msg->item_name.c_str());
1597 data[0] = (uint8_t) msg->value[i];
1600 data[0] =
DXL_LOBYTE((uint16_t )msg->value[i]);
1601 data[1] =
DXL_HIBYTE((uint16_t )msg->value[i]);
1619 if (msg->data ==
"DirectControlMode")
1624 it.second->rxPacket();
1628 else if (msg->data ==
"MotionModuleMode")
1632 it.second->txPacket();
1645 for (
int i = 0; i < msg->name.size(); i++)
1656 uint8_t sync_write_data[4];
1674 std::string _module_name_to_set = msg->data;
1688 if (msg->joint_name.size() != msg->module_name.size())
1698 robotis_controller_msgs::GetJointModule::Response &res)
1700 for (
unsigned int idx = 0; idx < req.joint_name.size(); idx++)
1702 auto d_it =
robot_->
dxls_.find((std::string) (req.joint_name[idx]));
1705 res.joint_name.push_back(req.joint_name[idx]);
1706 res.module_name.push_back(d_it->second->ctrl_module_name_);
1710 if (res.joint_name.size() == 0)
1721 robotis_controller_msgs::JointCtrlModule modules;
1722 modules.joint_name = req.joint_name;
1723 modules.module_name = req.module_name;
1725 robotis_controller_msgs::JointCtrlModule::ConstPtr msg_ptr(
new robotis_controller_msgs::JointCtrlModule(modules));
1727 if (modules.joint_name.size() != modules.module_name.size())
1742 std::string _module_name_to_set = req.module_name;
1754 std::list<MotionModule *> _stop_modules;
1755 std::list<MotionModule *> _enable_modules;
1757 for(
unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
1760 std::map<std::string, Dynamixel*>::iterator _dxl_it =
robot_->
dxls_.find((std::string)(msg->joint_name[idx]));
1762 _dxl = _dxl_it->second;
1771 if((*_stop_m_it)->getModuleName() == _dxl->
ctrl_module_name_ && (*_stop_m_it)->getModuleEnable() ==
true)
1772 _stop_modules.push_back(*_stop_m_it);
1778 _stop_modules.unique();
1779 for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
1781 (*_stop_m_it)->stop();
1785 for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
1787 while((*_stop_m_it)->isRunning())
1792 for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
1794 (*_stop_m_it)->setModuleEnable(
false);
1800 for(
unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
1802 std::string ctrl_module = msg->module_name[idx];
1803 std::string joint_name = msg->joint_name[idx];
1806 std::map<std::string, Dynamixel*>::iterator _dxl_it =
robot_->
dxls_.find(joint_name);
1808 _dxl = _dxl_it->second;
1813 if(ctrl_module ==
"" || ctrl_module ==
"none")
1821 uint8_t _sync_write_data[4];
1841 if((*_m_it)->getModuleName() == ctrl_module)
1843 std::map<std::string, DynamixelState*>::iterator _result_it = (*_m_it)->result_.find(joint_name);
1844 if(_result_it == (*_m_it)->result_.end())
1850 _enable_modules.push_back(*_m_it);
1859 uint8_t _sync_write_data[4];
1876 uint8_t _sync_write_data[4];
1893 uint8_t _sync_write_data[4];
1914 _enable_modules.unique();
1915 for(std::list<MotionModule *>::iterator _m_it = _enable_modules.begin(); _m_it != _enable_modules.end(); _m_it++)
1917 (*_m_it)->setModuleEnable(
true);
1926 robotis_controller_msgs::JointCtrlModule _current_module_msg;
1927 for(std::map<std::string, Dynamixel *>::iterator _dxl_iter =
robot_->
dxls_.begin(); _dxl_iter !=
robot_->
dxls_.end(); ++_dxl_iter)
1929 _current_module_msg.joint_name.push_back(_dxl_iter->first);
1930 _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name_);
1933 if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size())
1940 std::list<MotionModule *> stop_modules;
1942 if (ctrl_module ==
"" || ctrl_module ==
"none")
1947 if ((*m_it)->getModuleEnable() ==
true)
1948 stop_modules.push_back(*m_it);
1956 if ((*m_it)->getModuleName() == ctrl_module)
1959 for (
auto& result_it : (*m_it)->result_)
1966 if (d_it->second->ctrl_module_name_ != ctrl_module)
1970 if (((*stop_m_it)->getModuleName() == d_it->second->ctrl_module_name_) &&
1971 ((*stop_m_it)->getModuleEnable() ==
true))
1973 stop_modules.push_back(*stop_m_it);
1986 stop_modules.unique();
1987 for (
auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++)
1989 (*stop_m_it)->stop();
1993 for (
auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++)
1995 while ((*stop_m_it)->isRunning())
2000 for(std::list<MotionModule *>::iterator _stop_m_it = stop_modules.begin(); _stop_m_it != stop_modules.end(); _stop_m_it++)
2002 (*_stop_m_it)->setModuleEnable(
false);
2013 if ((ctrl_module ==
"") || (ctrl_module ==
"none"))
2025 uint8_t sync_write_data[4] = { 0 };
2046 if ((*m_it)->getModuleName() == ctrl_module)
2049 for (
auto& result_it : (*m_it)->result_)
2063 uint8_t sync_write_data[4] = { 0 };
2080 uint8_t sync_write_data[4] = { 0 };
2097 uint8_t sync_write_data[4] = { 0 };
2124 if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName())
2126 (*m_it)->setModuleEnable(
true);
2138 robotis_controller_msgs::JointCtrlModule current_module_msg;
2141 current_module_msg.joint_name.push_back(dxl_iter.first);
2142 current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_);
2145 if (current_module_msg.joint_name.size() == current_module_msg.module_name.size())
2153 for (
unsigned int i = 0; i < msg->name.size(); i++)
2155 auto d_it =
robot_->
dxls_.find((std::string) msg->name[i]);
2158 d_it->second->dxl_state_->present_position_ = msg->position[i];
2159 d_it->second->dxl_state_->present_velocity_ = msg->velocity[i];
2160 d_it->second->dxl_state_->present_torque_ = msg->effort[i];
2167 it.second->dxl_state_->goal_position_ = it.second->dxl_state_->present_position_;
2179 ROS_WARN(
"Process Timer is running.. STOP the timer first.");
2187 return ping(joint_name, 0, error);
2201 return pkt_handler->
ping(port_handler, dxl->
id_, model_number, error);
2216 return pkt_handler->
action(port_handler, dxl->
id_);
2230 return pkt_handler->
reboot(port_handler, dxl->
id_, error);
2244 return pkt_handler->
factoryReset(port_handler, dxl->
id_, option, error);
2259 return pkt_handler->
readTxRx(port_handler, dxl->
id_, address, length, data, error);
2283 uint8_t read_data = 0;
2291 uint16_t read_data = 0;
2299 uint32_t read_data = 0;
2323 return pkt_handler->
read1ByteTxRx(port_handler, dxl->
id_, address, data, error);
2338 return pkt_handler->
read2ByteTxRx(port_handler, dxl->
id_, address, data, error);
2353 return pkt_handler->
read4ByteTxRx(port_handler, dxl->
id_, address, data, error);
2368 return pkt_handler->
writeTxRx(port_handler, dxl->
id_, address, length, data, error);
2391 write_data[0] = (uint8_t) data;
2408 delete[] write_data;
2424 return pkt_handler->
write1ByteTxRx(port_handler, dxl->
id_, address, data, error);
2439 return pkt_handler->
write2ByteTxRx(port_handler, dxl->
id_, address, data, error);
2454 return pkt_handler->
write4ByteTxRx(port_handler, dxl->
id_, address, data, error);
2470 return pkt_handler->
regWriteTxRx(port_handler, dxl->
id_, address, length, data, error);
void setCtrlModuleThread(std::string ctrl_module)
virtual int read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error=0)=0
std::list< SensorModule * > sensor_modules_
void initializeDevice(const std::string init_file_path)
int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data, uint8_t *error=0)
boost::thread gazebo_thread_
int read(const std::string joint_name, uint16_t address, uint16_t length, uint8_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)
int reboot(const std::string joint_name, 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 publish(const boost::shared_ptr< M > &message) const
virtual int regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)=0
double convertValue2Torque(int16_t value)
std::string gazebo_robot_name_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ControlTableItem * present_velocity_item_
virtual int writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)=0
void initializeSyncWrite()
std::map< std::string, ros::Publisher > gazebo_joint_position_pub_
double convertValue2Radian(int32_t value)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_position_i_gain_
void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg)
ControlTableItem * position_p_gain_item_
ControllerMode controller_mode_
std::map< std::string, ros::Publisher > gazebo_joint_effort_pub_
ControlTableItem * velocity_p_gain_item_
virtual int read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error=0)=0
TimeStamp update_time_stamp_
boost::thread set_module_thread_
virtual int reboot(PortHandler *port, uint8_t id, uint8_t *error=0)=0
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void removeMotionModule(MotionModule *module)
std::map< std::string, ros::Publisher > gazebo_joint_velocity_pub_
void addMotionModule(MotionModule *module)
double convertValue2Velocity(int32_t value)
std::map< std::string, dynamixel::PortHandler * > ports_
std::vector< ControlTableItem * > bulk_read_items_
std::string ctrl_module_name_
std::map< std::string, Dynamixel * > dxls_
ros::Publisher goal_joint_state_pub_
void setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg)
int write2Byte(const std::string joint_name, uint16_t address, uint16_t data, uint8_t *error=0)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_velocity_d_gain_
boost::mutex queue_mutex_
TimeStamp update_time_stamp_
ControlTableItem * present_position_item_
std::map< std::string, std::string > port_default_device_
ControlTableItem * goal_current_item_
int32_t convertRadian2Value(double radian)
std::map< std::string, double > sensor_result_
virtual int readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)=0
int write1Byte(const std::string joint_name, uint16_t address, uint8_t data, uint8_t *error=0)
bool setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_position_p_gain_
bool initialize(const std::string robot_file_path, const std::string init_file_path)
bool setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res)
virtual void initialize(const int control_cycle_msec, Robot *robot)=0
void setCallbackQueue(CallbackQueueInterface *queue)
void loadOffset(const std::string path)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_current_
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_position_d_gain_
virtual int write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error=0)=0
ControlTableItem * goal_position_item_
ros::Publisher current_module_pub_
ControlTableItem * torque_enable_item_
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_velocity_i_gain_
std::list< MotionModule * > motion_modules_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int32_t convertVelocity2Value(double velocity)
static PacketHandler * getPacketHandler(float protocol_version=2.0)
int action(const std::string joint_name)
ControlTableItem * position_i_gain_item_
void setControllerModeCallback(const std_msgs::String::ConstPtr &msg)
ControlTableItem * velocity_i_gain_item_
int write4Byte(const std::string joint_name, uint16_t address, uint32_t data, uint8_t *error=0)
ControlTableItem * position_d_gain_item_
ROSLIB_DECL std::string getPath(const std::string &package_name)
virtual void initialize(const int control_cycle_msec, Robot *robot)=0
std::map< std::string, uint32_t > bulk_read_table_
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_velocity_
std::map< std::string, Sensor * > sensors_
std::string getModuleName()
SensorState * sensor_state_
virtual int write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error=0)=0
void addSensorModule(SensorModule *module)
#define ROS_INFO_STREAM(args)
std::string getModuleName()
std::map< std::string, dynamixel::GroupBulkRead * > port_to_bulk_read_
DynamixelState * dxl_state_
void syncWriteItemCallback(const robotis_controller_msgs::SyncWriteItem::ConstPtr &msg)
virtual int write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error=0)=0
virtual int action(PortHandler *port, uint8_t id)=0
int ping(const std::string joint_name, uint8_t *error=0)
static void * timerThread(void *param)
ControlTableItem * goal_velocity_item_
virtual int read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error=0)=0
int16_t convertTorque2Value(double torque)
void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
virtual int getBaudRate()=0
int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data, uint8_t *error=0)
ControlTableItem * velocity_d_gain_item_
virtual int ping(PortHandler *port, uint8_t id, uint8_t *error=0)=0
void removeSensorModule(SensorModule *module)
std::vector< dynamixel::GroupSyncWrite * > direct_sync_write_
void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg)
#define INDIRECT_ADDRESS_1
std::map< std::string, uint32_t > bulk_read_table_
ros::Publisher present_joint_state_pub_
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
virtual int factoryReset(PortHandler *port, uint8_t id, uint8_t option=0, uint8_t *error=0)=0
ControlTableItem * present_current_item_
virtual bool setBaudRate(const int baudrate)=0
int writeCtrlItem(const std::string joint_name, const std::string item_name, uint32_t data, uint8_t *error=0)
bool getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res)
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_position_
std::map< std::string, dynamixel::GroupSyncWrite * > port_to_sync_write_velocity_p_gain_
boost::thread queue_thread_
std::map< std::string, ControlTableItem * > ctrl_table_
int factoryReset(const std::string joint_name, uint8_t option=0, uint8_t *error=0)
void writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg)
int readCtrlItem(const std::string joint_name, const std::string item_name, uint32_t *data, uint8_t *error=0)
void setCtrlModule(std::string module_name)
int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data, uint8_t *error=0)