pose_interpolator.cpp
Go to the documentation of this file.
1 
18 //Global includes
19 
20 
21 
22 //Pkg includes
23 #include <ros/ros.h>
24 
25 
26 //ISM includes
27 #include <ISM/tools/PoseInterpolator.hpp>
28 
29 
30 bool getNodeParameters(ros::NodeHandle nh, std::string& source_file, std::string& target_file, int& step_number)
31 {
32  bool success = true;
33 
34  if (!nh.getParam("source", source_file) || source_file.empty())
35  {
36  ROS_INFO("Missing parameter: \"source\"");
37  success = false;
38  }
39  else
40  {
41  ROS_INFO_STREAM("source: " << source_file);
42  }
43 
44  if (!nh.getParam("target", target_file) || target_file.empty())
45  {
46  ROS_INFO("Missing parameter: \"target\"");
47  success = false;
48  }
49  else
50  {
51  ROS_INFO_STREAM("target: " << target_file);
52  }
53 
54  if (!nh.getParam("step_number", step_number))
55  {
56  ROS_INFO("Missing parameter: \"step_number\"");
57  success = false;
58  }
59  else
60  {
61  ROS_INFO_STREAM("step_number: " << step_number);
62  }
63 
64  return success;
65 }
66 
67 int main (int argc, char **argv)
68 {
69  //Usual ros node stuff
70  ros::init(argc, argv, "pose_interpolator");
71  ros::NodeHandle nh("~");
72  ISM::PoseInterpolator pose_interpolator;
73 
74  std::string target_file;
75  std::string source_file;
76  int step_number;
77 
78  if(getNodeParameters(nh, source_file, target_file, step_number))
79  {
80  if(step_number > 0)
81  {
82  pose_interpolator.interpolate(source_file, target_file, step_number);
83  ROS_INFO("INTERPOLATION COMPLETED!");
84  }
85  else
86  {
87  ROS_INFO("CHECK step_number! It should have a value of 1 or greater.");
88  }
89  }
90  else
91  {
92  ROS_INFO("INTERPOLATION ABORTED! Check launch-file for missing parameter.");
93  }
94 
95  return 0;
96 }
bool getNodeParameters(ros::NodeHandle nh, std::string &source_file, std::string &target_file, int &step_number)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_INFO(...)
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
int main(int argc, char **argv)


asr_ism
Author(s): Borella Jocelyn, Hanselmann Fabian, Heller Florian, Heizmann Heinrich, Kübler Marcel, Mehlhaus Jonas, Meißner Pascal, Qattan Mohamad, Reckling Reno, Stroh Daniel
autogenerated on Thu Jan 9 2020 07:20:58