50 #include <std_msgs/Float32.h> 60 #include "seed_r7_ros_controller/ResetRobotStatus.h" 65 #include <std_msgs/String.h> 66 #include <diagnostic_updater/diagnostic_updater.h> 83 void writeWheel(
const std::vector< std::string> &_names,
84 const std::vector<int16_t> &_vel,
double _tm_sec);
88 void runHandScript(uint8_t _number, uint16_t _script, uint8_t _current);
89 void turnWheel(std::vector<int16_t> &_vel);
94 void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat);
111 bool resetRobotStatusCallback(seed_r7_ros_controller::ResetRobotStatus::Request& _req, seed_r7_ros_controller::ResetRobotStatus::Response& _res);
void writeWheel(const std::vector< std::string > &_names, const std::vector< int16_t > &_vel, double _tm_sec)
void turnWheel(std::vector< int16_t > &_vel)
int BASE_COMMAND_PERIOD_MS_
hardware_interface::JointStateInterface js_interface_
void runLedScript(uint8_t _number, uint16_t _script)
void onWheelServo(bool _value)
std::vector< double > joint_effort_limits_
bool resetRobotStatusCallback(seed_r7_ros_controller::ResetRobotStatus::Request &_req, seed_r7_ros_controller::ResetRobotStatus::Response &_res)
void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
unsigned int number_of_angles_
boost::shared_ptr< seed_converter::StrokeConverter > stroke_converter_
ros::Publisher bat_vol_pub_
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_
boost::shared_ptr< robot_hardware::UpperController > controller_upper_
struct robot_hardware::RobotHW::RobotStatus robot_status_
std::vector< std::string > joint_names_upper_
std::vector< std::string > joint_names_lower_
boost::shared_ptr< robot_hardware::LowerController > controller_lower_
std::vector< double > joint_position_
pluginlib::ClassLoader< seed_converter::StrokeConverter > converter_loader_
std::vector< double > prev_ref_strokes_
std::vector< double > joint_effort_
std::vector< double > joint_position_command_
std::vector< double > joint_effort_command_
void getBatteryVoltage(const ros::TimerEvent &_event)
hardware_interface::PositionJointInterface pj_interface_
std::vector< JointType > joint_types_
std::vector< ControlMethod > joint_control_methods_
std::vector< int16_t > lower_act_strokes_
void readPos(const ros::Time &time, const ros::Duration &period, bool update)
std::vector< int16_t > upper_act_strokes_
void update(controller_manager::ControllerManager &cm, const ros::TimerEvent &e)
ros::ServiceServer reset_robot_status_server_
diagnostic_updater::Updater diagnostic_updater_
ros::Timer bat_vol_timer_
std::vector< double > joint_velocity_
std::vector< std::string > joint_list_
std::vector< double > joint_velocity_command_
std::string robot_model_plugin_
virtual void read(const ros::Time &time, const ros::Duration &period)
void runHandScript(uint8_t _number, uint16_t _script, uint8_t _current)
virtual void write(const ros::Time &time, const ros::Duration &period)
virtual bool init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh)