fts_config.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group,
00004  * Institute for Anthropomatics and Robotics (IAR) -
00005  * Intelligent Process Control and Robotics (IPR),
00006  * Karlsruhe Institute of Technology
00007  *
00008  * Author: Denis Štogl, email: denis.stogl@kit.edu
00009  *
00010  * Date of creation: 2014-2016
00011  *
00012  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00013  *
00014  * Redistribution and use in source and binary forms,
00015  * with or without modification, are permitted provided
00016  * that the following conditions are met:
00017  *
00018  *     * Redistributions of source code must retain the above copyright
00019  *       notice, this list of conditions and the following disclaimer.
00020  *     * Redistributions in binary form must reproduce the above copyright
00021  *       notice, this list of conditions and the following disclaimer in the
00022  *       documentation and/or other materials provided with the distribution.
00023  *     * Neither the name of the copyright holder nor the names of its
00024  *       contributors may be used to endorse or promote products derived from
00025  *       this software without specific prior written permission.
00026  *
00027  * This program is free software: you can redistribute it and/or modify
00028  * it under the terms of the GNU Lesser General Public License as published by
00029  * the Free Software Foundation, either version 3 of the License, or
00030  * (at your option) any later version.
00031  *
00032  * This package is distributed in the hope that it will be useful,
00033  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00034  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00035  * GNU Lesser General Public License for more details.
00036  *
00037  * You should have received a copy of the GNU Lesser General Public License
00038  * along with this package. If not, see <http://www.gnu.org/licenses/>.
00039  ****************************************************************/
00040 
00041 #include <stdint.h>
00042 typedef unsigned char uint8_t;
00043 #include <inttypes.h>
00044 #include <iostream>
00045 #include <ros/ros.h>
00046 #include <cob_forcetorque/ForceTorqueCtrl.h>
00047 
00048 #include <std_srvs/Trigger.h>
00049 
00050 #include <math.h>
00051 #include <iostream>
00052 #define PI 3.14159265
00053 
00054 class ForceTorqueConfig
00055 {
00056 public:
00057   ForceTorqueConfig();
00058 
00059   bool initFts();
00060   bool srvCallback_SetBaudRate(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
00061   bool srvCallback_SetBaseIdentifier(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
00062   bool srvCallback_Reset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
00063   // create a handle for this node, initialize node
00064   ros::NodeHandle nh_;
00065 
00066 private:
00067   // CAN parameters
00068   int canType;
00069   std::string canPath;
00070   int canBaudrate;
00071   int ftsBaseID;
00072   int ftsFutureBaudrate;
00073   int ftsFutureBaseID;
00074 
00075   // service servers
00076   ros::ServiceServer srvServer_SetBaudRate_;
00077   ros::ServiceServer srvServer_SetBaseIdentifier_;
00078   ros::ServiceServer srvSever_Reset_;
00079 
00080   bool m_isInitialized;
00081   ForceTorqueCtrl *p_Ftc;
00082 };
00083 
00084 ForceTorqueConfig::ForceTorqueConfig()
00085 {
00086   m_isInitialized = false;
00087 
00088   srvServer_SetBaudRate_ = nh_.advertiseService("SetBaudRate", &ForceTorqueConfig::srvCallback_SetBaudRate, this);
00089   srvServer_SetBaseIdentifier_ =
00090       nh_.advertiseService("SetBaseIdentifier", &ForceTorqueConfig::srvCallback_SetBaseIdentifier, this);
00091   srvSever_Reset_ = nh_.advertiseService("Reset", &ForceTorqueConfig::srvCallback_Reset, this);
00092 
00093   // Read data from parameter server
00094   nh_.param<int>("CAN/type", canType, -1);
00095   nh_.param<std::string>("CAN/path", canPath, "");
00096   nh_.param<int>("CAN/baudrate", canBaudrate, -1);
00097   nh_.param<int>("FTS/base_identifier", ftsBaseID, -1);
00098   nh_.param<int>("FTS/future_baudrate", ftsFutureBaudrate, ATI_CAN_BAUD_250K);
00099   nh_.param<int>("FTS/future_base_id", ftsFutureBaseID, 0x20);
00100 
00101   if (canType != -1)
00102   {
00103     p_Ftc = new ForceTorqueCtrl(canType, canPath, canBaudrate, ftsBaseID);
00104   }
00105   else
00106   {
00107     p_Ftc = new ForceTorqueCtrl();
00108   }
00109 
00110   initFts();
00111 }
00112 
00113 bool ForceTorqueConfig::initFts()
00114 {
00115   if (!m_isInitialized)
00116   {
00117     // read return init status and check it!
00118     if (p_Ftc->Init())
00119     {
00120       ROS_INFO("FTC initialized");
00121       m_isInitialized = true;
00122     }
00123     else
00124     {
00125       m_isInitialized = false;
00126       ROS_INFO("FTC initialisation failed");
00127     }
00128   }
00129   return m_isInitialized;
00130 }
00131 
00132 bool ForceTorqueConfig::srvCallback_SetBaudRate(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00133 {
00134   if (m_isInitialized)
00135   {
00136     if (p_Ftc->SetBaudRate(ftsFutureBaudrate))
00137     {
00138       ROS_INFO("New baud rate successfully set to: %d", ftsFutureBaudrate);
00139 
00140       res.success = true;
00141       res.message = "All good, you are nice person! :)";
00142     }
00143     else
00144     {
00145       res.success = false;
00146       res.message = "Could not set baud rate :)";
00147     }
00148   }
00149   else
00150   {
00151     res.success = false;
00152     res.message = "FTS not initialised! :/";
00153   }
00154 
00155   return true;
00156 }
00157 
00158 bool ForceTorqueConfig::srvCallback_SetBaseIdentifier(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00159 {
00160   if (m_isInitialized)
00161   {
00162     if (p_Ftc->SetBaseIdentifier(ftsFutureBaseID))
00163     {
00164       ROS_INFO("New base identifier successfully set to HEX: %x", ftsFutureBaseID);
00165 
00166       res.success = true;
00167       res.message = "All good, you are nice person! :)";
00168     }
00169     else
00170     {
00171       res.success = false;
00172       res.message = "Could not set base identifier :)";
00173     }
00174   }
00175   else
00176   {
00177     res.success = false;
00178     res.message = "FTS not initialised! :/";
00179   }
00180 
00181   return true;
00182 }
00183 
00184 bool ForceTorqueConfig::srvCallback_Reset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00185 {
00186   ROS_WARN("Going to reset NETCANOEM!");
00187 
00188   if (p_Ftc->Reset())
00189   {
00190     res.success = true;
00191     res.message = "Reset succeded!";
00192   }
00193   else
00194   {
00195     res.success = false;
00196     res.message = "Reset NOT succeded!";
00197   }
00198 
00199   return true;
00200 }
00201 
00202 int main(int argc, char **argv)
00203 {
00204   ros::init(argc, argv, "forcetorque_config");
00205   ForceTorqueConfig ftn;
00206 
00207   ROS_INFO("ForceTorque Config Node running.");
00208 
00209   ros::spin();
00210 
00211   return 0;
00212 }


ati_force_torque
Author(s): Denis Štogl, Alexander Bubeck
autogenerated on Thu Jun 6 2019 21:13:29