ttl_driver_node.cpp
Go to the documentation of this file.
1 /*
2  ttl_driver_node.cpp
3  Copyright (C) 2020 Niryo
4  All rights reserved.
5  This program is free software: you can redistribute it and/or modify
6  it under the terms of the GNU General Public License as published by
7  the Free Software Foundation, either version 3 of the License, or
8  (at your option) any later version.
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 General Public License for more details.
13  You should have received a copy of the GNU General Public License
14  along with this program. If not, see <http:// www.gnu.org/licenses/>.
15 */
16 
17 // ros
18 #include <memory>
19 #include <ros/ros.h>
20 #include <string>
21 
22 // niryo
23 #include "ros/serialization.h"
24 #include "ttl_driver/ttl_interface_core.hpp"
25 
26 using ::common::model::BusProtocolEnum;
27 using ::common::model::DxlMotorState;
28 using ::common::model::EBusProtocol;
29 using ::common::model::EHardwareType;
30 using ::common::model::HardwareTypeEnum;
31 using ::common::model::StepperMotorState;
32 using ::std::string;
33 using ::std::to_string;
34 
35 // Method add joints
36 void addJointToTtlInterface(const std::shared_ptr<ttl_driver::TtlInterfaceCore>& ttl_interface)
37 {
38  size_t nb_joints = 0;
39 
40  ros::NodeHandle robot_hwnh("joints_interface");
41  // retrieve nb joints with checking that the config param exists for both name and id
42  while (robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/id") &&
43  robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/name") &&
44  robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/type") &&
45  robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/bus"))
46  nb_joints++;
47 
48  // connect and register joint state interface
49 
50  int currentIdDxl = 1;
51  int currentIdStepper = 1;
52 
53  for (size_t j = 0; j < nb_joints; j++)
54  {
55  ROS_DEBUG("Initialize stepper motors");
56  int joint_id_config = 0;
57  string joint_name;
58  string joint_type;
59  string joint_bus;
60 
61  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/id", joint_id_config);
62  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/name", joint_name);
63  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/type", joint_type);
64  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/bus", joint_bus);
65  HardwareTypeEnum eType = HardwareTypeEnum(joint_type.c_str());
66  BusProtocolEnum eBusProto = BusProtocolEnum(joint_bus.c_str());
67 
68  if (eType == EHardwareType::STEPPER || eType == EHardwareType::FAKE_STEPPER_MOTOR)
69  { // stepper
70  std::string currentStepperNamespace = "steppers/stepper_" + to_string(currentIdStepper);
71 
72  auto stepperState = std::make_shared<StepperMotorState>(joint_name, eType, common::model::EComponentType::JOINT,
73  eBusProto, static_cast<uint8_t>(joint_id_config));
74  if (stepperState)
75  {
76  double offsetPos = 0.0;
77  double gear_ratio = 1.0;
78  int direction = 1;
79  double max_effort = 0.0;
80  double default_home_position = 0.0;
81  double limit_position_min = 0.0;
82  double limit_position_max = 0.0;
83  double motor_ratio = 0.0;
84 
85  robot_hwnh.getParam(currentStepperNamespace + "/offset_position", offsetPos);
86  robot_hwnh.getParam(currentStepperNamespace + "/gear_ratio", gear_ratio);
87  robot_hwnh.getParam(currentStepperNamespace + "/direction", direction);
88  robot_hwnh.getParam(currentStepperNamespace + "/max_effort", max_effort);
89  robot_hwnh.getParam(currentStepperNamespace + "/default_home_position", default_home_position);
90  robot_hwnh.getParam(currentStepperNamespace + "/limit_position_min", limit_position_min);
91  robot_hwnh.getParam(currentStepperNamespace + "/limit_position_max", limit_position_max);
92  robot_hwnh.getParam(currentStepperNamespace + "/motor_ratio", motor_ratio);
93 
94  // acceleration and velocity profiles
95  common::model::VelocityProfile profile{};
96  int data{};
97  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_start"))
98  {
99  robot_hwnh.getParam(currentStepperNamespace + "/v_start", data);
100  profile.v_start = static_cast<uint32_t>(data);
101  }
102 
103  if (robot_hwnh.hasParam(currentStepperNamespace + "/a_1"))
104  {
105  robot_hwnh.getParam(currentStepperNamespace + "/a_1", data);
106  profile.a_1 = static_cast<uint32_t>(data);
107  }
108  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_1"))
109  {
110  robot_hwnh.getParam(currentStepperNamespace + "/v_1", data);
111  profile.v_1 = static_cast<uint32_t>(data);
112  }
113  if (robot_hwnh.hasParam(currentStepperNamespace + "/a_max"))
114  {
115  robot_hwnh.getParam(currentStepperNamespace + "/a_max", data);
116  profile.a_max = static_cast<uint32_t>(data);
117  }
118  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_max"))
119  {
120  robot_hwnh.getParam(currentStepperNamespace + "/v_max", data);
121  profile.v_max = static_cast<uint32_t>(data);
122  }
123  if (robot_hwnh.hasParam(currentStepperNamespace + "/d_max"))
124  {
125  robot_hwnh.getParam(currentStepperNamespace + "/d_max", data);
126  profile.d_max = static_cast<uint32_t>(data);
127  }
128  if (robot_hwnh.hasParam(currentStepperNamespace + "/d_1"))
129  {
130  robot_hwnh.getParam(currentStepperNamespace + "/d_1", data);
131  profile.d_1 = static_cast<uint32_t>(data);
132  }
133  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_stop"))
134  {
135  robot_hwnh.getParam(currentStepperNamespace + "/v_stop", data);
136  profile.v_stop = static_cast<uint32_t>(data);
137  }
138 
139  // add parameters
140  stepperState->setOffsetPosition(offsetPos);
141  stepperState->setGearRatio(gear_ratio);
142  stepperState->setDirection(static_cast<int8_t>(direction));
143  stepperState->setMaxEffort(max_effort);
144  stepperState->setDefaultHomePosition(default_home_position);
145  stepperState->setLimitPositionMax(limit_position_max);
146  stepperState->setLimitPositionMin(limit_position_min);
147  stepperState->setMotorRatio(motor_ratio);
148  stepperState->setVelocityProfile(profile);
149 
150  if (eBusProto == EBusProtocol::TTL)
151  ttl_interface->addJoint(stepperState);
152 
153  currentIdStepper++;
154  }
155  }
156  else if (eType != EHardwareType::UNKNOWN)
157  { // dynamixel
158  ROS_DEBUG("Initialize dxl motors");
159  auto dxlState = std::make_shared<DxlMotorState>(joint_name, eType, common::model::EComponentType::JOINT,
160  static_cast<uint8_t>(joint_id_config));
161  if (dxlState)
162  {
163  double offsetPos = 0.0;
164  int direction = 1;
165  int positionPGain = 0;
166  int positionIGain = 0;
167  int positionDGain = 0;
168  int velocityPGain = 0;
169  int velocityIGain = 0;
170  int FF1Gain = 0;
171  int FF2Gain = 0;
172  int velocityProfile = 0;
173  int accelerationProfile = 0;
174  double limit_position_min = 0.0;
175  double limit_position_max = 0.0;
176  double default_home_position = 0.0;
177 
178  std::string currentDxlNamespace = "dynamixels/dxl_" + to_string(currentIdDxl);
179 
180  robot_hwnh.getParam(currentDxlNamespace + "/offset_position", offsetPos);
181  robot_hwnh.getParam(currentDxlNamespace + "/direction", direction);
182 
183  robot_hwnh.getParam(currentDxlNamespace + "/position_P_gain", positionPGain);
184  robot_hwnh.getParam(currentDxlNamespace + "/position_I_gain", positionIGain);
185  robot_hwnh.getParam(currentDxlNamespace + "/position_D_gain", positionDGain);
186 
187  robot_hwnh.getParam(currentDxlNamespace + "/velocity_P_gain", velocityPGain);
188  robot_hwnh.getParam(currentDxlNamespace + "/velocity_I_gain", velocityIGain);
189 
190  robot_hwnh.getParam(currentDxlNamespace + "/FF1_gain", FF1Gain);
191  robot_hwnh.getParam(currentDxlNamespace + "/FF2_gain", FF2Gain);
192 
193  robot_hwnh.getParam(currentDxlNamespace + "/velocity_profile", velocityProfile);
194  robot_hwnh.getParam(currentDxlNamespace + "/acceleration_profile", accelerationProfile);
195 
196  robot_hwnh.getParam(currentDxlNamespace + "/default_home_position", default_home_position);
197  robot_hwnh.getParam(currentDxlNamespace + "/limit_position_min", limit_position_min);
198  robot_hwnh.getParam(currentDxlNamespace + "/limit_position_max", limit_position_max);
199 
200  dxlState->setOffsetPosition(offsetPos);
201  dxlState->setDirection(static_cast<int8_t>(direction));
202  dxlState->setDefaultHomePosition(default_home_position);
203 
204  dxlState->setPositionPGain(static_cast<uint32_t>(positionPGain));
205  dxlState->setPositionIGain(static_cast<uint32_t>(positionIGain));
206  dxlState->setPositionDGain(static_cast<uint32_t>(positionDGain));
207 
208  dxlState->setVelocityPGain(static_cast<uint32_t>(velocityPGain));
209  dxlState->setVelocityIGain(static_cast<uint32_t>(velocityIGain));
210 
211  dxlState->setFF1Gain(static_cast<uint32_t>(FF1Gain));
212  dxlState->setFF2Gain(static_cast<uint32_t>(FF2Gain));
213 
214  if (eBusProto == EBusProtocol::TTL)
215  ttl_interface->addJoint(dxlState);
216 
217  currentIdDxl++;
218  }
219  }
220  } // end for (size_t j = 0; j < nb_joints; j++)
221 }
222 
223 int main(int argc, char** argv)
224 {
225  ros::init(argc, argv, "ttl_driver_node");
226 
227  ROS_DEBUG("Launching ttl_driver_node");
228 
229  ros::NodeHandle nodeHandle("~");
230 
231  std::shared_ptr<ttl_driver::TtlInterfaceCore> ttl_node = std::make_shared<ttl_driver::TtlInterfaceCore>(nodeHandle);
232 
233  addJointToTtlInterface(ttl_node);
234 
235  ros::spin();
236  return 0;
237 }
ttl_interface_core.hpp
main
int main(int argc, char **argv)
Definition: ttl_driver_node.cpp:223
addJointToTtlInterface
void addJointToTtlInterface(const std::shared_ptr< ttl_driver::TtlInterfaceCore > &ttl_interface)
Definition: ttl_driver_node.cpp:36

ttl_driver
Author(s): Clement Cocquempot
autogenerated on Wed May 21 2025 08:30:14