sensor_alignment_tf_dyn.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 
3 // PCL specific includes
5 #include <pcl/point_cloud.h>
6 #include <pcl/point_types.h>
7 #include <pcl_ros/transforms.h>
8 
9 #include <dynamic_reconfigure/server.h>
10 #include <sick_scan/tf_dynConfig.h>
11 
14 #include <tf2_eigen/tf2_eigen.h>
15 
16 Eigen::Affine3d child_2_parent;
17 
19 {
20 public:
21  std::string parentFrame;
22  std::string childFrame;
23  double trans_x;
24  double trans_y;
25  double trans_z;
26  double trans_rot_x;
27  double trans_rot_y;
28  double trans_rot_z;
29 };
30 
32 
33 
34 // for setting region of interest
35 Eigen::Vector4f min_point;
36 Eigen::Vector4f max_point;
37 
39 
40 
41 void callback(sick_scan_fusion::tf_dynConfig &config, uint32_t level)
42 {
43  cfgTrans.parentFrame = config.parent_frame;
44  cfgTrans.childFrame = config.child_frame;
45 
46  cfgTrans.trans_x = config.tf_x;
47  cfgTrans.trans_y = config.tf_y;
48  cfgTrans.trans_z = config.tf_z;
49  cfgTrans.trans_rot_x = config.tf_rot_x * M_PI / 180.0;
50  cfgTrans.trans_rot_y = config.tf_rot_y * M_PI / 180.0;
51  cfgTrans.trans_rot_z = config.tf_rot_z * M_PI / 180.0;
52  }
53 
54 int main (int argc, char** argv)
55 {
56  // initialize
57  ros::init (argc, argv, "sick_scan_fusion_tf_dyn");
58  ros::NodeHandle nh;
59 
60  tf2_ros::TransformListener tf_listener(tf_buffer);
61 
62 
63  child_2_parent = Eigen::Affine3d::Identity();
64 
65  // dynamic reconfigure
66  dynamic_reconfigure::Server<sick_scan_fusion::tf_dynConfig> server;
67  dynamic_reconfigure::Server<sick_scan_fusion::tf_dynConfig>::CallbackType f;
68  f = boost::bind(&callback, _1, _2);
69  server.setCallback(f);
70 
71 
72  unsigned seq= 0;
73  ros::Rate loop_rate(100);
74  while (ros::ok())
75  {
76  if ((seq % 20) == 0)
77  {
78  printf("Par. %s Child: %s Trans x: %0.3lf\n", cfgTrans.childFrame.c_str(), cfgTrans.parentFrame.c_str(), cfgTrans.trans_x);
80  child_2_parent = Eigen::Affine3d::Identity();
81  child_2_parent.translate(Eigen::Vector3d( cfgTrans.trans_x,0,0));
82  child_2_parent.translate(Eigen::Vector3d(0, cfgTrans.trans_y,0));
83  child_2_parent.translate(Eigen::Vector3d(0, 0, cfgTrans.trans_z));
84  // pitch
85  child_2_parent.rotate(Eigen::AngleAxisd(cfgTrans.trans_rot_x, Eigen::Vector3d(1, 0, 0)));
86  // yaw
87  child_2_parent.rotate(Eigen::AngleAxisd(cfgTrans.trans_rot_y, Eigen::Vector3d(0, 1, 0)));
88  // yaw
89  child_2_parent.rotate(Eigen::AngleAxisd(cfgTrans.trans_rot_z, Eigen::Vector3d(0, 0, 1)));
90 
91  geometry_msgs::TransformStamped transform_base_trans = tf2::eigenToTransform(child_2_parent);
92  transform_base_trans.header.frame_id = cfgTrans.parentFrame;
93  transform_base_trans.child_frame_id = cfgTrans.childFrame;
94  transform_base_trans.header.stamp = ros::Time::now();
95  transform_base_trans.header.seq = seq;
96  br.sendTransform(transform_base_trans);
97 
98  ros::spinOnce();
99  loop_rate.sleep();
100  ++seq;
101  }
102 }
103 
Eigen::Affine3d child_2_parent
ConfigTrans cfgTrans
int main(int argc, char **argv)
tf2_ros::Buffer tf_buffer
f
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
Eigen::Vector4f max_point
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Eigen::Vector4f min_point
void callback(sick_scan_fusion::tf_dynConfig &config, uint32_t level)
ROSCPP_DECL bool ok()
bool sleep()
static Time now()
ROSCPP_DECL void spinOnce()


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed Sep 7 2022 02:25:06