system_info.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
18 
19 #include <XmlRpcValue.h>
20 #include <XmlRpcException.h>
21 #include <sensor_msgs/JointState.h>
22 
25 #include <canopen_chain_node/GetObject.h>
27 
28 namespace prbt_hardware_support
29 {
30 
31 static const std::string CANOPEN_GETOBJECT_SERVICE_NAME{"/prbt/driver/get_object"};
32 static const std::string CANOPEN_NODES_PARAMETER_NAME{"/prbt/driver/nodes"};
33 static const std::string JOINT_STATE_TOPIC {"/joint_states"};
34 
35 static const std::string GET_FIRMWARE_VERION_OBJECT{"100A"};
36 
37 // Currently the string is defined to be 41 characters long, but the last character can be omitted.
38 // This is currently under investigation. See https://github.com/PilzDE/pilz_robots/issues/299.
39 static constexpr std::size_t FIRMWARE_STRING_LENGTH{40};
40 
42  : joint_names_( getNodeNames(nh) )
43 {
44  // Wait till CAN is up and running.
45  // Reason: If the first CAN service call happens before
46  // the CAN is fully initialized, the CAN will not start properly.
47  pilz_utils::waitForMessage<sensor_msgs::JointState>(JOINT_STATE_TOPIC);
48 
50  canopen_srv_get_client_ = nh.serviceClient<canopen_chain_node::GetObject>(CANOPEN_GETOBJECT_SERVICE_NAME);
51 }
52 
53 std::string SystemInfo::getFirmwareVersionOfJoint(const std::string& joint_name)
54 {
55  canopen_chain_node::GetObject srv;
56  srv.request.node = joint_name;
57  srv.request.object = GET_FIRMWARE_VERION_OBJECT;
58  srv.request.cached = false;
59 
60  ROS_INFO_STREAM("Call \"get firmware\" service for \"" << joint_name << "\"");
61  if (!canopen_srv_get_client_.call(srv))
62  {
63  throw SystemInfoException("CANopen service to request firmware version failed");
64  }
65 
66  if (!srv.response.success)
67  {
68  throw SystemInfoException(srv.response.message);
69  }
70 
71  srv.response.value.resize(FIRMWARE_STRING_LENGTH);
72 
73  return srv.response.value;
74 }
75 
77 {
78  FirmwareCont versions;
79  for(const auto& joint : joint_names_)
80  {
81  versions[joint] = getFirmwareVersionOfJoint(joint);
82  }
83  return versions;
84 }
85 
86 std::vector<std::string> SystemInfo::getNodeNames(const ros::NodeHandle& nh)
87 {
90  {
91  throw SystemInfoException("Could not read node names");
92  }
93 
94  std::vector<std::string> node_names;
95  for (auto& rpci : rpc)
96  {
97  auto node_name = rpci.first.c_str();
98  node_names.push_back(node_name);
99  }
100  return node_names;
101 }
102 
103 } // namespace prbt_hardware_support
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
const std::vector< std::string > joint_names_
Definition: system_info.h:63
static std::vector< std::string > getNodeNames(const ros::NodeHandle &nh)
Definition: system_info.cpp:86
bool call(MReq &req, MRes &res)
bool getParam(const std::string &key, std::string &s) const
static void waitForService(const std::string service_name, const double retry_timeout=DEFAULT_RETRY_TIMEOUT, const double msg_output_period=DEFAULT_MSG_OUTPUT_PERIOD)
SystemInfo(ros::NodeHandle &nh)
Definition: system_info.cpp:41
static const std::string CANOPEN_NODES_PARAMETER_NAME
static const std::string JOINT_STATE_TOPIC
Definition: system_info.cpp:33
static const std::string GET_FIRMWARE_VERION_OBJECT
Definition: system_info.cpp:35
static constexpr std::size_t FIRMWARE_STRING_LENGTH
Definition: system_info.cpp:39
#define ROS_INFO_STREAM(args)
static const std::string CANOPEN_GETOBJECT_SERVICE_NAME
std::string getFirmwareVersionOfJoint(const std::string &joint_name)
Definition: system_info.cpp:53
Exception thrown by the SystemInfo class in case of an error.
ros::ServiceClient canopen_srv_get_client_
Definition: system_info.h:64
std::map< std::string, std::string > FirmwareCont
Definition: system_info.h:34


prbt_hardware_support
Author(s):
autogenerated on Fri Feb 28 2020 03:16:57