viz.cpp
Go to the documentation of this file.
1 #include <rosflight_utils/viz.h>
2 
3 namespace rosflight_utils
4 {
5 Viz::Viz() : nh_private_("~")
6 {
7  // retrieve params
8 
9  // initialize variables
10  mag_sum_ = 0;
11  mag_skip_ = 20;
12  mag_count_ = 0;
13  mag_throttle_ = 0;
14 
15  // Magnetometer visualization
16  mag_sub_ = nh_.subscribe("/magnetometer", 1, &Viz::magCallback, this);
17  mag_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("viz/magnetometer", 1);
18  pts_pub_ = nh_.advertise<visualization_msgs::Marker>("viz/cloud", 1);
19 
20  // Attitude visualization
21  att_sub_ = nh_.subscribe("/attitude", 1, &Viz::attCallback, this);
22  pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("viz/attitude", 1);
23 }
24 
25 void Viz::magCallback(const sensor_msgs::MagneticFieldConstPtr &msg)
26 {
28  {
29  // unpack message
30  double x = msg->magnetic_field.x;
31  double y = msg->magnetic_field.y;
32  double z = msg->magnetic_field.z;
33 
34  // get euler angles from vector (assume no roll)
35  double yaw = atan2(y, x);
36  double pitch = atan2(-z, sqrt(x * x + y * y));
37 
38  // convert to body quaternion and rotation into the vehicle frame
39  tf::Quaternion q_v = tf::createQuaternionFromRPY(0, pitch, yaw);
40 
41  // pack data into pose message
42  geometry_msgs::PoseStamped pose_msg;
43  pose_msg.header = msg->header;
44  pose_msg.header.frame_id = fixed_frame_;
45  pose_msg.pose.position.x = 0;
46  pose_msg.pose.position.y = 0;
47  pose_msg.pose.position.z = 0;
48  tf::quaternionTFToMsg(q_v, pose_msg.pose.orientation);
49 
50  // Publish the messages
51  mag_pub_.publish(pose_msg);
52 
53  // MEASUREMENT CLOUD //
54 
55  // store the current measurement
56  geometry_msgs::Point p;
57  p.x = x;
58  p.y = y;
59  p.z = z;
60  pts_list_.push_back(p);
61 
62  // begin packing marker message
63  visualization_msgs::Marker pts_msg;
64  pts_msg.header.frame_id = fixed_frame_;
65  pts_msg.header.stamp = msg->header.stamp;
66  pts_msg.type = visualization_msgs::Marker::POINTS;
67  pts_msg.action = visualization_msgs::Marker::ADD;
68 
69  // set points style
70  pts_msg.scale.x = 0.1;
71  pts_msg.scale.y = pts_msg.scale.x;
72  pts_msg.scale.z = pts_msg.scale.x;
73  pts_msg.color.a = 1.0;
74  pts_msg.color.r = 0.0;
75  pts_msg.color.g = 1.0;
76  pts_msg.color.b = 0.0;
77 
78  for (uint32_t i = 0; i < pts_list_.size(); ++i)
79  {
80  pts_msg.points.push_back(pts_list_[i]);
81  }
82 
83  // publish point cloud
84  pts_pub_.publish(pts_msg);
85  }
86  mag_throttle_++;
87 }
88 
89 void Viz::attCallback(const rosflight_msgs::AttitudeConstPtr &msg)
90 {
91  geometry_msgs::PoseStamped pose;
92  pose.header = msg->header;
93  pose.header.frame_id = fixed_frame_;
94  pose.pose.orientation = msg->attitude;
95 
96  pose_pub_.publish(pose);
97 }
98 
99 } // namespace rosflight_utils
100 
101 int main(int argc, char **argv)
102 {
103  ros::init(argc, argv, "viz_node");
105  ros::spin();
106  return 0;
107 }
ros::NodeHandle nh_
Definition: viz.h:23
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
ros::Subscriber att_sub_
Definition: viz.h:32
void magCallback(const sensor_msgs::MagneticFieldConstPtr &msg)
Definition: viz.cpp:25
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher pose_pub_
Definition: viz.h:33
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
float atan2(float y, float x)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROSCPP_DECL void spin(Spinner &spinner)
ros::Publisher pts_pub_
Definition: viz.h:29
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
This file defines a simple PID controller to be used by other classes to implement a PID control loop...
Definition: simple_pid.h:44
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string fixed_frame_
Definition: viz.h:40
ros::Publisher mag_pub_
Definition: viz.h:28
TFSIMD_FORCE_INLINE const tfScalar & z() const
int i
int main(int argc, char **argv)
Definition: viz.cpp:101
void attCallback(const rosflight_msgs::AttitudeConstPtr &msg)
Definition: viz.cpp:89
std::vector< geometry_msgs::Point > pts_list_
Definition: viz.h:37
int mag_throttle_
Definition: viz.h:39
double mag_sum_
Definition: viz.h:38
ros::Subscriber mag_sub_
Definition: viz.h:27


rosflight_utils
Author(s):
autogenerated on Thu Apr 15 2021 05:10:06