23 #include <sensor_msgs/JointState.h> 24 #include <control_msgs/FollowJointTrajectoryAction.h> 32 #include <boost/thread/mutex.hpp> 33 #include <boost/shared_ptr.hpp> 34 #include <boost/scoped_ptr.hpp> 47 sensor_msgs::JointState
g_js;
54 boost::scoped_ptr<FollowJointTrajectoryActionServer>
g_as;
55 control_msgs::FollowJointTrajectoryGoalConstPtr
g_goal;
73 return g_status && (g_status->status & (g_status->MASK_FINGER_READY | g_status->MASK_ERROR)) == g_status->MASK_FINGER_READY && g_status->rc == 0;
77 control_msgs::FollowJointTrajectoryResult result;
79 bool goal_reached =
false;
82 for(
size_t i = 0; i < g_status->joints.position_cdeg.size(); ++i){
85 result.error_code = result.GOAL_TOLERANCE_VIOLATED;
91 g_deadline_timer.
stop();
94 g_deadline_timer.
stop();
95 g_as->setSucceeded(result);
96 }
else if (deadline_exceeded) {
97 g_deadline_timer.
stop();
98 result.error_code = result.GOAL_TOLERANCE_VIOLATED;
99 g_as->setAborted(result,
"goal not reached in time");
108 ROS_ERROR(
"Halt service did not succeed");
119 g_port = nh_priv.
param<std::string>(
"sdhx/port",
"/dev/ttyACM0");
121 nh_priv.
param(
"sdhx/min_pwm0", 0),
122 nh_priv.
param(
"sdhx/min_pwm1", 0),
123 nh_priv.
param(
"sdhx/max_pwm0", 0),
124 nh_priv.
param(
"sdhx/max_pwm1", 0)) ){
127 g_status->status &= ~g_status->MASK_ERROR;
136 if((g_status->status & g_status->MASK_ERROR) == 0 && g_status->rc == 0) {
138 }
else if(
g_sdhx->isInitialized()) {
142 g_status->status &= ~g_status->MASK_ERROR;
153 boost::mutex::scoped_lock lock(
g_mutex);
156 res.
message =
"hand is not yet connected";
157 }
else if(g_status->status == g_status->NOT_INITIALIZED) {
166 res.
message =
"Not initialized";
174 res.
message =
"finger already initialized, restarting the controller";
178 res.
message =
"already initialized";
185 boost::mutex::scoped_lock lock(
g_mutex);
187 res.
message =
"hand is not yet connected";
196 boost::mutex::scoped_lock lock(
g_mutex);
198 res.
message =
"hand is not yet connected";
207 boost::mutex::scoped_lock lock(
g_mutex);
210 bool calc_vel = !first && dt != 0 ;
214 g_status->status = g_status->NOT_INITIALIZED;
219 int16_t position_cdeg[2];
220 int16_t velocity_cdeg_s[2];
221 int16_t current_100uA[2];
222 if(
g_sdhx->getData(position_cdeg, velocity_cdeg_s, current_100uA, boost::chrono::seconds(1))){
223 g_status->status |= g_status->MASK_FINGER_READY;
226 g_status->status |= g_status->MASK_ERROR;
230 g_status->rc =
g_sdhx->getRC();
233 g_topic_status->tick(g_status->stamp);
238 if(g_status->status & g_status->MASK_FINGER_READY){
239 for(
size_t i=0; i < g_status->joints.position_cdeg.size(); ++i){
242 g_js.velocity[i] = (new_pos -
g_js.position[i]) / dt;
251 g_js.position[i] = new_pos;
253 g_js.header.stamp = g_status->stamp;
264 boost::mutex::scoped_lock lock(
g_mutex);
267 stat.
summary(stat.ERROR,
"not connected");
271 if(g_status->status == g_status->NOT_INITIALIZED){
272 stat.
summary(stat.WARN,
"not initialized");
273 }
else if(g_status->status & g_status->MASK_ERROR){
274 stat.
summary(stat.ERROR,
"Bridge has error");
276 stat.
summary(stat.OK,
"Connected and running");
278 stat.
add(
"sdhx_ready",
bool(g_status->status & g_status->MASK_FINGER_READY));
279 stat.
add(
"sdhx_rc", uint32_t(g_status->rc));
283 if(g_status->rc > 0){
289 boost::mutex::scoped_lock lock(
g_mutex);
290 if(
g_as->isActive()){
301 control_msgs::FollowJointTrajectoryGoalConstPtr goal =
g_as->acceptNewGoal();
303 g_deadline_timer.
stop();
306 control_msgs::FollowJointTrajectoryResult result;
307 result.error_code = result.INVALID_GOAL;
308 if(goal->trajectory.points.size()!=1 && (goal->trajectory.points.size()!=2 || !goal->trajectory.points[0].time_from_start.isZero() ) ){
309 g_as->setAborted(result,
"goal is not valid");
316 for(
size_t i=0; i <
g_js.name.size(); ++i){
317 for(
size_t j=0; j < goal->trajectory.joint_names.size(); ++j){
318 if(
g_js.name[i] == goal->trajectory.joint_names[j]){
321 if(goal->trajectory.points.back().effort.size() > 0){
322 if(goal->trajectory.points.back().effort.size() == new_command.
current_100uA.size()){
323 new_command.
current_100uA[i] = goal->trajectory.points.back().effort[j] * 1000.0;
325 g_as->setAborted(result,
"Number of effort values mismatch");
336 if(found !=
g_js.name.size()){
337 g_as->setAborted(result,
"Joint names mismatch");
342 for(
size_t i = 0; i < goal->goal_tolerance.size(); ++i){
344 for(
size_t j=0; j <
g_js.name.size(); ++j){
345 if(goal->goal_tolerance[i].name ==
g_js.name[j]){
347 if(goal->goal_tolerance[i].position > 0.0){
348 goal_tolerance[j] = goal->goal_tolerance[i].position;
354 g_as->setAborted(result,
"Goal tolerance invalid");
360 ros::Time trajectory_deadline = (goal->trajectory.header.stamp.isZero() ? now : goal->trajectory.header.stamp)
361 + goal->trajectory.points.back().time_from_start +
ros::Duration(goal->goal_time_tolerance);
362 if(trajectory_deadline <= now){
363 result.error_code = result.OLD_HEADER_TIMESTAMP;
364 g_as->setAborted(result,
"goal is not valid");
368 boost::mutex::scoped_lock lock(
g_mutex);
371 g_as->setAborted(result,
"SDHx is not ready for commands");
375 g_command = new_command;
379 g_command_timer.
stop();
382 int16_t position_cdeg[2];
383 int16_t velocity_cdeg_s[2];
384 int16_t current_100uA[2];
388 g_sdhx->move(position_cdeg, velocity_cdeg_s, current_100uA);
392 g_deadline_timer.
start();
393 g_command_timer.
start();
397 boost::mutex::scoped_lock lock(
g_mutex);
399 g_deadline_timer.
stop();
403 g_as->setPreempted();
407 boost::mutex::scoped_lock lock(
g_mutex);
411 int16_t position_cdeg[2];
412 int16_t velocity_cdeg_s[2];
413 int16_t current_100uA[2];
417 g_sdhx->move(position_cdeg, velocity_cdeg_s, current_100uA);
420 g_command_timer.
stop();
423 ROS_WARN(
"finger is not ready, stopped resend timer");
427 int main(
int argc,
char* argv[])
429 ros::init(argc, argv,
"cob_hand_bridge_node");
436 ROS_ERROR(
"Please provide joint names for SDHx");
452 double stopped_current= nh_priv.
param(
"sdhx/stopped_current", 0.1);
453 if(stopped_current <= 0.0){
459 std::vector<double> default_currents;
460 if(nh_priv.
getParam(
"sdhx/default_currents", default_currents)){
461 if(default_currents.size() != g_default_command.
current_100uA.size()){
462 ROS_ERROR_STREAM(
"Number of current values does not match number of joints");
465 for(
size_t i=0; i< g_default_command.
current_100uA.size(); ++i){
466 g_default_command.
current_100uA[i] = default_currents[i] * 1000.0;
484 nh_priv.
param(
"status/min_duration", -1.0),
485 nh_priv.
param(
"status/max_duration", 0.1)
492 double resend_period = nh_priv.
param(
"sdhx/resend_period", 0.1);
497 g_js_pub = nh.
advertise<sensor_msgs::JointState>(
"joint_states", 1);
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val)
std::vector< double > g_goal_tolerance
void publish(const boost::shared_ptr< M > &message) const
actionlib::SimpleActionServer< control_msgs::FollowJointTrajectoryAction > FollowJointTrajectoryActionServer
void summary(unsigned char lvl, const std::string s)
cob_hand_bridge::JointValues g_command
void setHardwareID(const std::string &hwid)
void setPeriod(const Duration &period, bool reset=true)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::scoped_ptr< FollowJointTrajectoryActionServer > g_as
void add(const std::string &name, TaskFunction f)
NodeHandle_< EmbeddedLinuxHardware > NodeHandle
void handleDeadline(const ros::TimerEvent &)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
int main(int argc, char *argv[])
ros::Timer g_deadline_timer
bool haltCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
ros::Timer g_command_timer
static double from_degrees(double degrees)
int16_t velocity_cdeg_s[2]
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
bool isFingerReady_nolock()
ros::ServiceServer g_halt_srv
boost::scoped_ptr< SDHX > g_sdhx
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
double g_stopped_velocity
void resendCommand(const ros::TimerEvent &e)
static double to_degrees(double radians)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
boost::shared_ptr< diagnostic_updater::TimeStampStatus > g_topic_status
ros::ServiceServer g_recover_srv
void mergeSummary(unsigned char lvl, const std::string s)
void reportDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
bool initCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
ros::ServiceServer g_init_srv
bool getParam(const std::string &key, std::string &s) const
control_msgs::FollowJointTrajectoryGoalConstPtr g_goal
sensor_msgs::JointState g_js
void statusCallback(const ros::TimerEvent &e)
bool recoverCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
cob_hand_bridge::JointValues g_default_command
void add(const std::string &key, const T &val)
bool advertiseService(ServiceServer< MReq, MRes > &srv)
#define ROS_ERROR_STREAM(args)
bool checkAction_nolock(bool deadline_exceeded)
boost::shared_ptr< cob_hand_bridge::Status > g_status