CAN_compatibility_arm.cpp
Go to the documentation of this file.
1 
30 
31 #include <time.h>
32 #include <ros/ros.h>
33 #include <std_msgs/Float64.h>
34 #include <string>
35 #include <vector>
36 
37 namespace shadowrobot
38 {
40 
41  SRArticulatedRobot(), n_tilde("~")
42  {
43  ROS_WARN(
44  "This interface is deprecated, you should probably use the interface provided by the CAN driver directly.");
45 
46  initializeMap();
47 
49  }
50 
52  {
53  }
54 
56  {
57  joints_map_mutex.lock();
58  JointData tmpData;
59 
60  std::string controller_suffix;
61  std::string searched_param;
62  n_tilde.searchParam("controller_suffix", searched_param);
63  n_tilde.param(searched_param, controller_suffix, std::string("position_controller"));
64  std::string topic_prefix = "/sa_";
65  std::string topic_suffix = "_" + controller_suffix + "/command";
66  std::string full_topic = "";
67 
68  tmpData.min = -45.0;
69  tmpData.max = 45.0;
70 
71  full_topic = topic_prefix + "sr" + topic_suffix;
72  CAN_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
73  int tmp_index = 0;
74  tmpData.publisher_index = tmp_index;
75  joints_map["ShoulderJRotate"] = tmpData;
76 
77  tmpData.min = 0.0;
78  tmpData.max = 80.0;
79 
80  full_topic = topic_prefix + "ss" + topic_suffix;
81  CAN_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
82  tmp_index++;
83  tmpData.publisher_index = tmp_index;
84  joints_map["ShoulderJSwing"] = tmpData;
85 
86  tmpData.min = 20.0;
87  tmpData.max = 120.0;
88  full_topic = topic_prefix + "es" + topic_suffix;
89  CAN_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
90  tmp_index++;
91  tmpData.publisher_index = tmp_index;
92  joints_map["ElbowJSwing"] = tmpData;
93 
94  tmpData.min = -80.0;
95  tmpData.max = 80.0;
96 
97  full_topic = topic_prefix + "er" + topic_suffix;
98  CAN_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
99  tmp_index++;
100  tmpData.publisher_index = tmp_index;
101  joints_map["ElbowJRotate"] = tmpData;
102 
103  joints_map_mutex.unlock();
104  }
105 
106  int16_t CANCompatibilityArm::sendupdate(std::string joint_name, double target)
107  {
108  if (!joints_map_mutex.try_lock())
109  {
110  return -1;
111  }
112  JointsMap::iterator iter = joints_map.find(joint_name);
113  std_msgs::Float64 target_msg;
114 
115  // not found
116  if (iter == joints_map.end())
117  {
118  ROS_DEBUG("Joint %s not found", joint_name.c_str());
119 
120  joints_map_mutex.unlock();
121  return -1;
122  }
123 
124  // joint found
125  JointData tmpData(iter->second);
126 
127  if (target < tmpData.min)
128  {
129  target = tmpData.min;
130  }
131  if (target > tmpData.max)
132  {
133  target = tmpData.max;
134  }
135 
136  tmpData.target = target;
137 
138  joints_map[joint_name] = tmpData;
139  // the targets are in radians
140  target_msg.data = sr_math_utils::to_rad(target);
141 
142  // ROS_ERROR("Joint %s ,pub index %d, pub name %s", joint_name.c_str(),tmpData.publisher_index,
143  // CAN_publishers[tmpData.publisher_index].getTopic().c_str());
144  CAN_publishers[tmpData.publisher_index].publish(target_msg);
145 
146  joints_map_mutex.unlock();
147  return 0;
148  }
149 
151  {
152  JointData noData;
153  if (!joints_map_mutex.try_lock())
154  {
155  return noData;
156  }
157 
158  JointsMap::iterator iter = joints_map.find(joint_name);
159 
160  // joint found
161  if (iter != joints_map.end())
162  {
163  JointData tmp = JointData(iter->second);
164 
165  joints_map_mutex.unlock();
166  return tmp;
167  }
168 
169  ROS_ERROR("Joint %s not found.", joint_name.c_str());
170  joints_map_mutex.unlock();
171  return noData;
172  }
173 
175  {
176  return joints_map;
177  }
178 
179  int16_t CANCompatibilityArm::setContrl(std::string contrlr_name, JointControllerData ctrlr_data)
180  {
181  ROS_WARN(
182  "The set Controller function is not implemented in the CAN compatibility wrapper,"
183  " please use the provided services directly.");
184  return 0;
185  }
186 
188  {
189  JointControllerData no_result;
190  ROS_WARN(
191  "The get Controller function is not implemented in the CAN compatibility wrapper,"
192  " please use the provided services directly.");
193  return no_result;
194  }
195 
196  int16_t CANCompatibilityArm::setConfig(std::vector <std::string> myConfig)
197  {
198  ROS_WARN("The set config function is not implemented.");
199  return 0;
200  }
201 
202  void CANCompatibilityArm::getConfig(std::string joint_name)
203  {
204  ROS_WARN("The get config function is not implemented.");
205  }
206 
207  std::vector <DiagnosticData> CANCompatibilityArm::getDiagnostics()
208  {
209  std::vector <DiagnosticData> returnVect;
210  return returnVect;
211  }
212 
213  void CANCompatibilityArm::joint_states_callback(const sensor_msgs::JointStateConstPtr &msg)
214  {
215  if (!joints_map_mutex.try_lock())
216  {
217  return;
218  }
219  // loop on all the names in the joint_states message
220  for (size_t index = 0; index < msg->name.size(); ++index)
221  {
222  std::string joint_name = msg->name[index];
223  JointsMap::iterator iter = joints_map.find(joint_name);
224 
225  // not found => can be a joint from the arm / hand
226  if (iter == joints_map.end())
227  {
228  continue;
229  }
230 
231  // joint found
232  JointData tmpData(iter->second);
233 
234  tmpData.position = sr_math_utils::to_degrees(msg->position[index]);
235  tmpData.force = msg->effort[index];
236  tmpData.velocity = msg->velocity[index];
237  joints_map[joint_name] = tmpData;
238  }
239 
240  joints_map_mutex.unlock();
241  }
242 
243 } // namespace shadowrobot
244 
245 
246 /* For the emacs weenies in the crowd.
247 Local Variables:
248  c-basic-offset: 2
249 End:
250 */
virtual JointControllerData getContrl(std::string ctrlr_name)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::map< std::string, JointData > JointsMap
virtual int16_t sendupdate(std::string joint_name, double target)
void joint_states_callback(const sensor_msgs::JointStateConstPtr &msg)
virtual std::vector< DiagnosticData > getDiagnostics()
static double to_degrees(double rad)
#define ROS_WARN(...)
static double to_rad(double degrees)
virtual JointData getJointData(std::string joint_name)
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
JointsMap joints_map
A mapping between the joint names and the information regarding those joints.
virtual int16_t setContrl(std::string contrlr_name, JointControllerData ctrlr_data)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_DEPRECATED
This is a parent class for the different types of Shadowhand we can have. It makes it possible to swa...
virtual void getConfig(std::string joint_name)
std::vector< ros::Publisher > CAN_publishers
#define ROS_ERROR(...)
virtual int16_t setConfig(std::vector< std::string > myConfig)
#define ROS_DEBUG(...)
This compatibility interface is a wrapper around the new CAN Hand ROS driver. It is used to present t...


sr_hand
Author(s): Ugo Cupcic
autogenerated on Tue Oct 13 2020 03:55:53