25 ROS_INFO (
"[armadillo2_hw/dxl_motors_builder]: done building motor id: %d, model: %d",
26 motor.id, motor.spec.model);
33 ROS_INFO(
"[armadillo2_hw/ricboard_pub]: ricboard is up");
38 ROS_WARN(
"[armadillo2_hw/dxl_motors_builder]: dxl motors hardware is disabled");
68 speakMsg(
"dxl motors read error", 1);
69 ROS_ERROR(
"[dxl_motors_builder]: too many read errors, shutting down...");
82 if (motor.id == motor_id)
84 std::vector<dxl::motor> single_motor_vec;
85 motor.command_position = position;
86 motor.command_velocity = velocity;
87 single_motor_vec.push_back(motor);
88 write(single_motor_vec);
112 speakMsg(
"dxl motors write error", 1);
113 ROS_ERROR(
"[dxl_motors_builder]: too many write errors, shutting down...");
133 int error_counter = 0;
137 ROS_WARN(
"[dxl_motors_builder]: pinging motor id: %d failed", motor.id);
140 speakMsg(
"too many dxl motor ping errors, shutting down", 1);
141 ROS_ERROR(
"[dxl_motors_builder]: too many ping errors, motor %d is not responding. \n" 142 "check if motor crashed (red blink) and try to restart. \n" 143 "also make sure LATENCY_TIMER is set to 1 in dynamixel_sdk, and that the appropriate" 144 "rule for dynamixel port is existed with LATENCY_TIMER=1.", motor.id);
164 ROS_ERROR(
"[dxl_motors_builder]: motor spec at index %d param data type is invalid or missing. " 165 "make sure that this param exist in dxl_spec_config.yaml and that your launch includes this param file. shutting down...", i);
175 ROS_ERROR(
"[dxl_motors_builder]: spec name at index %d: invalid data type or missing. " 176 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
185 ROS_ERROR(
"[dxl_motors_builder]: spec model at index %d: invalid data type or missing. " 186 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
195 ROS_ERROR(
"[dxl_motors_builder]: spec cpr at index %d: invalid data type or missing. " 196 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
205 ROS_ERROR(
"[dxl_motors_builder]: spec rpm_factor at index %d: invalid data type or missing. " 206 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
215 ROS_ERROR(
"[dxl_motors_builder]: spec torque_const_a at index %d: invalid data type or missing. " 216 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
225 ROS_ERROR(
"[dxl_motors_builder]: spec torque_const_b at index %d: invalid data type or missing. " 226 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
237 ROS_ERROR(
"[dxl_motors_builder]: spec pos_read_addr at index %d: invalid data type or missing. " 238 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
247 ROS_ERROR(
"[dxl_motors_builder]: spec vel_read_addr at index %d: invalid data type or missing. " 248 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
258 ROS_ERROR(
"[dxl_motors_builder]: spec current_read_addr at index %d: invalid data type or missing. " 259 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
268 ROS_ERROR(
"[dxl_motors_builder]: spec error_read_addr at index %d: invalid data type or missing. " 269 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
278 ROS_ERROR(
"[dxl_motors_builder]: spec torque_write_addr at index %d: invalid data type or missing. " 279 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
288 ROS_ERROR(
"[dxl_motors_builder]: spec vel_write_addr at index %d: invalid data type or missing. " 289 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
299 ROS_ERROR(
"[dxl_motors_builder]: spec pos_write_addr at index %d: invalid data type or missing. " 300 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
309 ROS_ERROR(
"[dxl_motors_builder]: spec current_ratio at index %d: invalid data type or missing. " 310 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
319 ROS_ERROR(
"[dxl_motors_builder]: spec len_present_speed at index %d: invalid data type or missing. " 320 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
329 ROS_ERROR(
"[dxl_motors_builder]: spec len_present_pos at index %d: invalid data type or missing. " 330 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
339 ROS_ERROR(
"[dxl_motors_builder]: spec len_present_curr at index %d: invalid data type or missing. " 340 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
349 ROS_ERROR(
"[dxl_motors_builder]: spec len_goal_speed at index %d: invalid data type or missing. " 350 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
359 ROS_ERROR(
"[dxl_motors_builder]: spec len_goal_pos at index %d: invalid data type or missing. " 360 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
372 bool spec_found =
specs_.find(motor.spec.model) !=
specs_.end();
375 motor.spec.name =
specs_[motor.spec.model].name;
376 motor.spec.cpr =
specs_[motor.spec.model].cpr;
377 motor.spec.rpm_scale_factor =
specs_[motor.spec.model].rpm_scale_factor;
378 motor.spec.torque_const_a =
specs_[motor.spec.model].torque_const_a;
379 motor.spec.torque_const_b =
specs_[motor.spec.model].torque_const_b;
382 motor.spec.pos_read_addr =
specs_[motor.spec.model].pos_read_addr;
383 motor.spec.vel_read_addr =
specs_[motor.spec.model].vel_read_addr;
384 motor.spec.current_read_addr =
specs_[motor.spec.model].current_read_addr;
385 motor.spec.error_read_addr =
specs_[motor.spec.model].error_read_addr;
388 motor.spec.torque_write_addr =
specs_[motor.spec.model].torque_write_addr;
389 motor.spec.vel_write_addr =
specs_[motor.spec.model].vel_write_addr;
390 motor.spec.pos_write_addr =
specs_[motor.spec.model].pos_write_addr;
391 motor.spec.len_present_speed =
specs_[motor.spec.model].len_present_speed;
392 motor.spec.len_present_pos =
specs_[motor.spec.model].len_present_pos;
393 motor.spec.len_present_curr =
specs_[motor.spec.model].len_present_curr;
394 motor.spec.len_goal_speed =
specs_[motor.spec.model].len_goal_speed;
395 motor.spec.len_goal_pos =
specs_[motor.spec.model].len_goal_pos;
399 motor.spec.current_ratio =
specs_[motor.spec.model].current_ratio;
404 ROS_ERROR(
"[dxl_motors_builder]: couldn't locate model specification for motor %d, model %d. " 405 "make sure dxl_motor_data.yaml contains all the necessary specs. shutting down...", motor.id, motor.spec.model);
417 std::string str_flag = flag ?
"on" :
"off";
424 ROS_WARN(
"[dxl_motors_builder]: failed set torque of motor id %d to %s", motor.id, str_flag.c_str());
430 ROS_INFO(
"[dxl_motors_builder]: motors torque is %s", str_flag.c_str());
437 std_srvs::SetBool::Response &res)
455 ROS_ERROR(
"[dxl_motors_builder]: %s param is missing on param server. make sure that this param exist in dxl_joints_config.yaml " 463 ROS_ERROR(
"[dxl_motors_builder]: %s param is invalid (need to be an of type array) or missing. " 464 "make sure that this param exist in dxl_joints_config.yaml and that your launch " 473 ROS_ERROR(
"[dxl_motors_builder]: %s param is missing on param server. make sure that this param exist in dxl_joints_config.yaml " 474 "and that your launch includes this param file. shutting down...",
SPEC_CONFIG_PARAM);
481 ROS_ERROR(
"[dxl_motors_builder]: %s param is invalid (need to be an of type array) or missing. " 482 "make sure that this param exist in dxl_joints_config.yaml and that your launch " 491 ROS_ERROR(
"[dxl_motors_builder]: %s param is missing on param server. make sure that this param exist in dxl_joints_config.yaml " 492 "and that your launch includes this param file. shutting down...",
DXL_PROTOCOL_PARAM);
500 ROS_ERROR(
"[dxl_motors_builder]: %s param is missing on param server. make sure that this param exist in dxl_joints_config.yaml " 501 "and that your launch includes this param file. shutting down...",
DXL_PORT_PARAM);
509 ROS_ERROR(
"[dxl_motors_builder]: %s param is missing on param server. make sure that this param exist in dxl_joints_config.yaml " 526 ROS_ERROR(
"[dxl_motors_builder]: dxl motor id at index %d param data type is invalid or missing. " 527 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
543 ROS_ERROR(
"[dxl_motors_builder]: dxl motor id at index %d: invalid data type or missing. " 544 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
552 ROS_ERROR(
"[dxl_motors_builder]: dxl motor joint_name at index %d: invalid data type or missing. " 553 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
561 ROS_ERROR(
"[dxl_motors_builder]: dxl motor interface_type at index %d: invalid data type or missing. " 562 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
566 std::string string_interface_type =
static_cast<std::string
>(
dxl_joints_config_[i][
"interface"]);
571 ROS_ERROR(
"[dxl_motors_builder]: dxl motor direction at index %d: invalid data type or missing. " 572 "make sure that this param exist in dxl_joints_config.yaml and that your launch includes this param file. shutting down...", i);
599 switch (motor.interface_type)
604 &motor.command_position));
610 &motor.command_position,
611 &motor.command_velocity));
628 ROS_ERROR(
"[dxl_motors_builder]: open dxl port %s failed. " 629 "make sure cable is connected and port has the right permissions. shutting down...",
dxl_port_.c_str());
637 ROS_ERROR(
"[dxl_motors_builder]: protocol version %f is invalid. shutting down...",
protocol_);
std::vector< hardware_interface::JointHandle > pos_handles_
std::vector< hardware_interface::JointStateHandle > joint_state_handles_
void registerHandles(hardware_interface::JointStateInterface &joint_state_interface, hardware_interface::PositionJointInterface &position_interface, hardware_interface::PosVelJointInterface &posvel_interface)
DxlMotorsBuilder(ros::NodeHandle &nh)
void speakMsg(std::string msg, int sleep_time)
InterfaceType interface_type
XmlRpc::XmlRpcValue dxl_joints_config_
bool setTorque(bool flag)
uint16_t torque_write_addr
uint16_t len_present_speed
uint16_t current_read_addr
bool readMotorsError(std::vector< motor > &motors)
bool dont_allow_zero_ticks_vel
#define DXL_PROTOCOL_PARAM
ros::Publisher espeak_pub_
#define SPEC_CONFIG_PARAM
bool readMotorsVel(std::vector< motor > &motors)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Type const & getType() const
std::map< uint16_t, dxl::spec > specs_
XmlRpc::XmlRpcValue dxl_spec_config_
std::vector< hardware_interface::PosVelJointHandle > posvel_handles_
ROSCPP_DECL bool get(const std::string &key, std::string &s)
void writeToMotor(int motor_id, double position, double velocity)
uint16_t len_present_curr
ros::ServiceServer torque_srv_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void registerHandle(const JointStateHandle &handle)
static InterfaceType stringToInterfaceType(std::string type)
bool bulkWritePosition(std::vector< motor > &motors)
JointStateHandle getHandle(const std::string &name)
bool readMotorsPos(std::vector< motor > &motors)
#define DXL_JOINTS_CONFIG_PARAM
bool getParam(const std::string &key, std::string &s) const
ROSCPP_DECL void shutdown()
dxl::DxlInterface dxl_interface_
bool hasParam(const std::string &key) const
std::vector< dxl::motor > motors_
bool torqueServiceCB(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
PortState openPort(std::string port_name, unsigned int baudrate, float protocol)
#define DXL_PORT_BAUD_PARAM
bool readMotorsLoad(std::vector< motor > &motors)
bool setTorque(motor &motor, bool flag)
bool bulkWriteVelocity(std::vector< motor > &motors)