49 std::string port_upper(
"/dev/aero_upper");
50 std::string port_lower(
"/dev/aero_lower");
53 if (robot_hw_nh.
hasParam(
"port_upper")) robot_hw_nh.
getParam(
"port_upper", port_upper);
54 if (robot_hw_nh.
hasParam(
"port_lower")) robot_hw_nh.
getParam(
"port_lower", port_lower);
57 else ROS_WARN(
"/joint_settings/upper read error");
59 else ROS_WARN(
"/joint_settings/lower read error");
60 if (robot_hw_nh.
hasParam(
"controller_rate")) {
62 robot_hw_nh.
getParam(
"controller_rate", rate);
67 if (robot_hw_nh.
hasParam(
"overlap_scale")) {
69 robot_hw_nh.
getParam(
"overlap_scale", scl);
75 ROS_INFO(
"upper_port: %s", port_upper.c_str());
76 ROS_INFO(
"lower_port: %s", port_lower.c_str());
95 std::string model_str;
96 if (!root_nh.
getParam(
"robot_description", model_str)) {
97 ROS_ERROR(
"Failed to get model from robot_description");
102 ROS_ERROR(
"Failed to parse robot_description");
106 ROS_DEBUG(
"read %d joints", number_of_angles_);
144 if (!urdf_limits_ok) {
145 ROS_WARN(
"urdf limits of joint %s is not defined", jointname.c_str());
164 std::thread t1([&](){
167 std::thread t2([&](){
177 std::vector<int16_t> act_strokes(0);
185 act_strokes.insert(act_strokes.end(),act_upper_strokes.begin(),act_upper_strokes.end());
186 act_strokes.insert(act_strokes.end(),act_lower_strokes.begin(),act_lower_strokes.end());
189 std::vector<double> act_positions;
193 else ROS_ERROR(
"Not defined robot model, please check robot_model_name");
196 double tm = period.
toSec();
198 float position = act_positions[j];
199 float velocity = 0.0;
254 std::vector<bool > mask_positions(number_of_angles_);
255 std::fill(mask_positions.begin(), mask_positions.end(),
true);
258 double tmp = ref_positions[i];
260 mask_positions[i] =
false;
269 else ROS_ERROR(
"Not defined robot model, please check robot_model_name");
271 std::vector<int16_t> snt_strokes(ref_strokes);
275 size_t aero_array_size = 30;
276 std::vector<int16_t> upper_strokes(aero_array_size);
277 std::vector<int16_t> lower_strokes(aero_array_size);
290 std::thread t1([&](){
293 std::thread t2([&](){
319 ROS_INFO(
"sendnum : %d, script : %d", _number, _script);
void registerInterface(T *iface)
unsigned int number_of_angles_
boost::shared_ptr< NoidLowerController > controller_lower_
boost::shared_ptr< NoidUpperController > controller_upper_
std::vector< std::string > joint_names_upper_
std::vector< std::string > joint_list_
std::vector< double > joint_effort_
URDF_EXPORT bool initString(const std::string &xmlstring)
hardware_interface::PositionJointInterface pj_interface_
std::vector< double > prev_ref_positions_
void Angle2Stroke(std::vector< int16_t > &_strokes, const std::vector< double > _angles)
std::vector< JointType > joint_types_
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_
std::vector< double > joint_position_command_
std::vector< double > joint_velocity_
void enforceLimits(const ros::Duration &period)
virtual void write(const ros::Time &time, const ros::Duration &period)
void writeWheel(const std::vector< std::string > &_names, const std::vector< int16_t > &_vel, double _tm_sec)
void readPos(const ros::Time &time, const ros::Duration &period, bool update)
void onWheelServo(bool _value)
void Stroke2Angle(std::vector< double > &_angles, const std::vector< int16_t > _strokes)
std::vector< ControlMethod > joint_control_methods_
void registerHandle(const JointStateHandle &handle)
std::vector< double > joint_effort_command_
void MaskRobotCommand(std::vector< int16_t > &_strokes, const std::vector< bool > _angles)
JointStateHandle getHandle(const std::string &name)
std::vector< double > joint_position_
hardware_interface::JointStateInterface js_interface_
bool getParam(const std::string &key, std::string &s) const
void runHandScript(uint8_t _number, uint16_t _script, uint8_t _current)
bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits &limits)
virtual bool init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh)
bool hasParam(const std::string &key) const
std::vector< double > joint_velocity_command_
std::vector< std::string > joint_names_lower_
virtual void read(const ros::Time &time, const ros::Duration &period)