52 : initialized_(false), sensors_(NULL)
60 ros_ethercat_model::RobotState* robot_state;
61 std::string robot_state_name;
62 controller_nh.
param<std::string>(
"robot_state_name", robot_state_name,
"unique_robot_hw");
69 robot_state = hw->getHandle(robot_state_name).getState();
73 ROS_ERROR_STREAM(
"Could not find robot state: " << robot_state_name <<
" Not loading the controller. " << e.
what());
77 if (!nh_priv.
getParam(
"use_ns", use_ns))
79 ROS_INFO(
"Private parameter 'use_ns' not set, default is using namespace");
105 sr_actuator::SrMotorActuator* motor_actuator =
static_cast<sr_actuator::SrMotorActuator*
>(
106 robot_state->getActuator(
prefix_+
"FFJ0"));
109 sensors_ = motor_actuator->motor_state_.tactiles_;
114 ROS_ERROR(
"Parameter 'publish_rate' not set");
141 else if (
sensors_->at(0).type ==
"biotac")
145 else if (
sensors_->at(0).type ==
"ubi")
virtual const char * what() const
virtual void stopping(const ros::Time &time)
virtual void starting(const ros::Time &time)
boost::shared_ptr< SrTactileSensorPublisher > sensor_publisher_
Publishes Biotac tactile state.
virtual void update(const ros::Time &time, const ros::Duration &period)
#define ROS_FATAL_STREAM(args)
std::vector< tactiles::AllTactileData > * sensors_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
virtual bool init(ros_ethercat_model::RobotStateInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
SrTactileSensorController()
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM(args)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
derived from ImuSensorController author: Adolfo Rodriguez Tsouroukdissian
ros::NodeHandle nh_prefix_
Publishes PST tactile state.