45 bool autostart, use_internal_logging;
49 std::string logging_config_file;
55 int manual_major_version_int = 0;
57 int manual_minor_version_int = 0;
61 nh.
param<
bool>(
"autostart", autostart,
false);
62 nh.
param<
bool>(
"use_internal_logging", use_internal_logging,
false);
65 nh.
getParam(
"disable_flags", disable_flags);
66 nh.
param<
int>(
"reset_timeout", reset_timeout, 5);
67 nh.
getParam(
"logging_config", logging_config_file);
70 nh.
getParam(
"VERSIONS_PARAMETERS", dynamic_parameters);
71 nh.
param<
int>(
"use_major_version", manual_major_version_int, 0);
72 manual_major_version =
static_cast<uint16_t>(manual_major_version_int);
73 nh.
param<
int>(
"use_minor_version", manual_minor_version_int, 0);
74 manual_minor_version =
static_cast<uint16_t>(manual_minor_version_int);
86 if (use_internal_logging)
102 ROS_INFO(
"Internal logging was activated, output will be written as configured in " 103 "logging.xml (default to ~/.ros/log)");
107 ROS_WARN(
"Internal logging was enabled but config file could not be read. Please make sure " 108 "the provided path to the config file is correct.");
116 for (
size_t i = 0; i < 9; ++i)
118 if (disable_flags[i])
129 if (manual_major_version == 0 && manual_minor_version == 0)
133 ROS_INFO(
"Hand hardware controller version: %d.%d",
147 DynamicParameter dyn_parameters(manual_major_version, manual_minor_version, dynamic_parameters);
156 fm_->setCurrentSettings(
157 static_cast<driver_svh::SVHChannel>(channel),
162 fm_->setPositionSettings(
163 static_cast<driver_svh::SVHChannel>(channel),
168 fm_->setHomeSettings(static_cast<driver_svh::SVHChannel>(channel),
177 ROS_ERROR(
"Parameter Error! While reading the controller settings. Will use default settings");
194 std_msgs::MultiArrayDimension dim;
195 dim.label =
"channel currents";
204 ROS_INFO(
"Driver was autostarted! Input can now be sent. Have a safe and productive day!");
208 ROS_INFO(
"SVH Driver Ready, you will need to connect and reset the fingers before you can use " 224 fm_->setResetSpeed(config.finger_reset_speed);
225 fm_->setResetTimeout(config.reset_timeout);
231 if (
fm_->isConnected())
239 "Could not connect to SCHUNK five finger hand with serial device %s, and retry count %i",
251 if (
fm_->resetChannel(svh_channel))
253 ROS_INFO(
"Channel %i successfully homed!", svh_channel);
257 ROS_ERROR(
"Could not reset channel %i !", svh_channel);
264 fm_->enableChannel(static_cast<driver_svh::SVHChannel>(channel->data));
278 std::vector<std::string>::const_iterator joint_name;
279 for (joint_name = input->name.begin(); joint_name != input->name.end(); ++joint_name, ++index)
284 bool valid_input =
false;
301 if (input->position.size() > index)
303 target_positions[channel] = input->position[index];
304 pos_read[channel] =
true;
309 ROS_WARN_STREAM(
"Vector of input joint state is too small! Cannot access element nr " 322 if (!
fm_->setAllTargetPositions(target_positions))
334 fm_->setTargetPosition(static_cast<driver_svh::SVHChannel>(i), target_positions[i], 0.0);
343 if (
fm_->isConnected())
348 double cur_pos = 0.0;
349 double cur_cur = 0.0;
350 if (
fm_->isHomed(static_cast<driver_svh::SVHChannel>(channel)))
352 fm_->getPosition(static_cast<driver_svh::SVHChannel>(channel), cur_pos);
354 fm_->getCurrent(static_cast<driver_svh::SVHChannel>(channel), cur_cur);
368 if (
fm_->isConnected())
373 double cur_cur = 0.0;
374 if (
fm_->isHomed(static_cast<driver_svh::SVHChannel>(channel)))
376 fm_->getCurrent(static_cast<driver_svh::SVHChannel>(channel), cur_cur);
391 int main(
int argc,
char** argv)
416 dynamic_reconfigure::Server<svh_controller::svhConfig> server;
417 dynamic_reconfigure::Server<svh_controller::svhConfig>::CallbackType
f;
420 server.setCallback(f);
436 nh.
subscribe<sensor_msgs::JointState>(
"channel_targets",
445 nh.
advertise<std_msgs::Float64MultiArray>(
"channel_currents", 1);
455 channel_pos_pub.
publish(svh_node.getChannelFeedback());
456 channel_current_pub.publish(svh_node.getChannelCurrents());
std_msgs::Float64MultiArray getChannelCurrents()
getChannelCurrents Returns the current values of the channels as raw output
SVHNode(const ros::NodeHandle &nh)
SVHNode constructs a new node object that handles most of the functionality.
std::string serial_device_name_
Serial device to use for communication with hardware.
bool initialize(int &argc, char *argv[], bool remove_read_arguments)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
char * strdup(const char *s)
void enableChannelCallback(const std_msgs::Int8ConstPtr &channel)
Callback function to enable channels of SCHUNK five finger hand.
The SVHCurrentSettings save the current controller paramters for a single motor.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::vector< bool > home_settings_given
void jointStateCallback(const sensor_msgs::JointStateConstPtr &input)
Callback function for setting channel target positions to SCHUNK five finger hand.
uint16_t version_minor
Minor version number.
void dynamic_reconfigure_callback(svh_controller::svhConfig &config, uint32_t level)
Dynamic reconfigure callback to update changing parameters.
std::vector< bool > position_settings_given
std::vector< std::vector< float > > position_settings
std::vector< std::vector< float > > home_settings
TransportHints & tcpNoDelay(bool nodelay=true)
std::vector< bool > current_settings_given
void resetChannelCallback(const std_msgs::Int8ConstPtr &channel)
Callback function to reset/home channels of SCHUNK five finger hand.
Class to read parameter file and set the correct hardware parameters.
#define ROS_WARN_ONCE(...)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
The SVHFirmwareInfo holds the data of a firmware response from the hardware.
uint16_t version_major
Major version number.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std_msgs::Float64MultiArray channel_currents
Current Value message template.
std::string name_prefix
Prefix for the driver to identify joint names if the Driver should expext "left_hand_Pinky" than the ...
static const char * m_channel_description[]
Description values to get the corresponding string value to a channel enum.
#define ROS_WARN_STREAM(args)
int connect_retry_count
Number of times the connect routine tries to connect in case that we receive at least one package...
sensor_msgs::JointState getChannelFeedback()
SVHNode::getChannelFeedback Gets the latest received positions and efforts from the driver...
sensor_msgs::JointState channel_pos_
joint state message template
void connectCallback(const std_msgs::Empty &)
Callback function for connecting to SCHUNK five finger hand.
const Settings & getSettings() const
The SVHPositionSettings save the position controller paramters for a single motor.
bool getParam(const std::string &key, std::string &s) const
static const float channel_effort_constants[9]
Effort multipliers to calculate the torque of the motors for the individual channels.
int main(int argc, char **argv)
boost::shared_ptr< driver_svh::SVHFingerManager > fm_
Handle to the SVH finger manager for library access.
data sctructure for home positions
ROSCPP_DECL void spinOnce()
SVHChannel
Channel indicates which motor to use in command calls. WARNING: DO NOT CHANGE THE ORDER OF THESE as i...
std::vector< std::vector< float > > current_settings