send_targets.cpp
Go to the documentation of this file.
1 
26 #include <ros/ros.h>
27 
28 // we store the publishers in a map.
29 #include <map>
30 #include <string>
31 #include <vector>
32 
33 // You need to send a Float64 message to the joints.
34 #include <std_msgs/Float64.h>
35 
37 {
38 public:
45  {
50  static const std::string joints[20] =
51  {
52  "ffj0", "ffj3", "ffj4",
53  "mfj0", "mfj3", "mfj4",
54  "rfj0", "rfj3", "rfj4",
55  "lfj0", "lfj3", "lfj4", "lfj5",
56  "thj1", "thj2", "thj3", "thj4", "thj5",
57  "wrj1", "wrj2"
58  };
59 
60  static const std::string prefix = "/sh_";
61  static const std::string suffix = "_position_controller/command";
62 
63  /*
64  * We now build the map of publishers: there's one topic per joint.
65  * to send a target to a joint, we need to publish on /sh_ffj3_position_controller/command
66  * (replacing ffj3 by the other joint names, or position controller by the controller
67  * you want to use: you can run rostopic list to see which topics are available).
68  *
69  * The topic type is Float64.
70  */
71  for (unsigned int index_joint = 0; index_joint < 20; ++index_joint)
72  {
73  std::string joint_tmp = joints[index_joint];
74  publisher_map_[joint_tmp] = node_handle_.advertise<std_msgs::Float64>(prefix + joint_tmp + suffix, 2);
75  }
76  };
77 
79  {
80  };
81 
90  bool sendupdate(std::string joint_name, double target)
91  {
92  // first we find the joint in the map (or return an error if we
93  // can't find it)
94  std::map<std::string, ros::Publisher>::iterator publisher_iterator;
95  publisher_iterator = publisher_map_.find(joint_name);
96 
97  if (publisher_iterator == publisher_map_.end())
98  {
99  ROS_WARN_STREAM("Joint " << joint_name << " not found.");
100  return false;
101  }
102 
103  // now we build the message:
104  // the target must be send in RADIANS
105  std_msgs::Float64 msg_to_send;
106  msg_to_send.data = target * 3.14159 / 360.0;
107  publisher_iterator->second.publish(msg_to_send);
108 
109  return true;
110  };
111 
112 protected:
114  std::map <std::string, ros::Publisher> publisher_map_;
117 };
118 
119 
132 int main(int argc, char **argv)
133 {
134  // Initialize the ROS node.
135  ros::init(argc, argv, "send_targets");
136 
137  // Instantiate the target sender
138  TargetSender target_sender = TargetSender();
139 
140  // Builds the vector of targets
141  static const unsigned int length_targets = 1000;
142  std::vector<double> targets;
143 
144  for (unsigned int i = 0; i < length_targets; ++i)
145  {
146  double target = static_cast<double>(i) / 1000.0 * 90.0;
147  targets.push_back(target);
148  }
149 
150  // Send the targets until the node is killed.
151  unsigned int step = 0;
152  while (ros::ok())
153  {
154  target_sender.sendupdate("ffj3", targets[step % length_targets]);
155  usleep(10000);
156 
157  ++step;
158  }
159 
160  return 0;
161 }
162 
163 /* For the emacs weenies in the crowd.
164 Local Variables:
165  c-basic-offset: 2
166 End:
167 */
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
ros::NodeHandle node_handle_
A node handle to be able to publish.
int main(int argc, char **argv)
std::map< std::string, ros::Publisher > publisher_map_
The map were the publishers are stored.
bool sendupdate(std::string joint_name, double target)


sr_edc_ethercat_drivers
Author(s): Ugo Cupcic, Yann Sionneau, Toni Oliver
autogenerated on Fri Jan 1 2021 03:23:23