rviz_visualization.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2020-2022 RaccoonLab.
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation, version 3.
7  *
8  * This program is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
17  */
18 
19 #include "rviz_visualization.hpp"
20 #include "cs_converter.hpp"
21 #include "vtolDynamicsSim.hpp"
22 
23 static const constexpr uint8_t PX4_NED_FRD = 0;
24 static const constexpr uint8_t ROS_ENU_FLU = 1;
25 
26 static const std::array<std::string, 5> MOTOR_NAMES = {"motor0", "motor1", "motor2", "motor3", "ICE"};
27 static const char GLOBAL_FRAME_ID[] = "world";
28 static const char UAV_FRAME_ID[] = "uav/enu";
29 static const char UAV_FIXED_FRAME_ID[] = "uav/com";
30 
31 RvizVisualizator::RvizVisualizator(ros::NodeHandle& nh) : node(nh) {
32 }
33 
34 int8_t RvizVisualizator::init(const std::shared_ptr<UavDynamicsSimBase>& uavDynamicsSim_) {
35  uavDynamicsSim = uavDynamicsSim_;
36 
37  if (uavDynamicsSim_ == nullptr) {
38  return -1;
39  }
40 
41  initMarkers();
42  totalForcePub = node.advertise<visualization_msgs::Marker>("/uav/Ftotal", 1);
43  aeroForcePub = node.advertise<visualization_msgs::Marker>("/uav/Faero", 1);
44  motorsForcesPub[0] = node.advertise<visualization_msgs::Marker>("/uav/Fmotor0", 1);
45  motorsForcesPub[1] = node.advertise<visualization_msgs::Marker>("/uav/Fmotor1", 1);
46  motorsForcesPub[2] = node.advertise<visualization_msgs::Marker>("/uav/Fmotor2", 1);
47  motorsForcesPub[3] = node.advertise<visualization_msgs::Marker>("/uav/Fmotor3", 1);
48  motorsForcesPub[4] = node.advertise<visualization_msgs::Marker>("/uav/Fmotor4", 1);
49  liftForcePub = node.advertise<visualization_msgs::Marker>("/uav/liftForce", 1);
50  drugForcePub = node.advertise<visualization_msgs::Marker>("/uav/drugForce", 1);
51  sideForcePub = node.advertise<visualization_msgs::Marker>("/uav/sideForce", 1);
52 
53  totalMomentPub = node.advertise<visualization_msgs::Marker>("/uav/Mtotal", 1);
54  aeroMomentPub = node.advertise<visualization_msgs::Marker>("/uav/Maero", 1);
55  controlSurfacesMomentPub = node.advertise<visualization_msgs::Marker>("/uav/McontrolSurfaces", 1);
56  aoaMomentPub = node.advertise<visualization_msgs::Marker>("/uav/Maoa", 1);
57  motorsMomentsPub[0] = node.advertise<visualization_msgs::Marker>("/uav/Mmotor0", 1);
58  motorsMomentsPub[1] = node.advertise<visualization_msgs::Marker>("/uav/Mmotor1", 1);
59  motorsMomentsPub[2] = node.advertise<visualization_msgs::Marker>("/uav/Mmotor2", 1);
60  motorsMomentsPub[3] = node.advertise<visualization_msgs::Marker>("/uav/Mmotor3", 1);
61  motorsMomentsPub[4] = node.advertise<visualization_msgs::Marker>("/uav/Mmotor4", 1);
62 
63  velocityPub = node.advertise<visualization_msgs::Marker>("/uav/linearVelocity", 1);
64 
65  return 0;
66 }
67 
68 
69 void RvizVisualizator::publish(uint8_t) {
70  arrowMarkers.header.stamp = ros::Time();
71  const Eigen::Vector3d MOMENT_COLOR(0.5, 0.5, 0.0);
72  const Eigen::Vector3d MOTORS_FORCES_COLOR(0.0, 0.5, 0.5);
73  const Eigen::Vector3d SPEED_COLOR(0.7, 0.5, 1.3);
74  const Eigen::Vector3d LIFT_FORCE(0.8, 0.2, 0.3);
75  const Eigen::Vector3d DRAG_FORCE(0.2, 0.8, 0.3);
76  const Eigen::Vector3d SIDE_FORCE(0.2, 0.3, 0.8);
77 
78  auto moments = dynamic_cast<VtolDynamics*>(uavDynamicsSim.get())->getMoments();
79  auto forces = dynamic_cast<VtolDynamics*>(uavDynamicsSim.get())->getForces();
80 
81  // publish moments
82  aeroMomentPub.publish(makeArrow(moments.aero, MOMENT_COLOR, UAV_FRAME_ID));
83 
84  for(size_t motorIdx = 0; motorIdx < 5; motorIdx++){
85  motorsMomentsPub[motorIdx].publish(makeArrow(moments.motors[motorIdx],
86  MOMENT_COLOR,
87  MOTOR_NAMES[motorIdx].c_str()));
88  }
89 
90  totalMomentPub.publish(makeArrow(moments.total, MOMENT_COLOR, UAV_FRAME_ID));
91 
92  controlSurfacesMomentPub.publish(makeArrow(moments.steer, MOMENT_COLOR, UAV_FRAME_ID));
93 
94  aoaMomentPub.publish(makeArrow(moments.airspeed, MOMENT_COLOR, UAV_FRAME_ID));
95 
96 
97 
98  // publish forces
99  aeroForcePub.publish(makeArrow(forces.aero / 10, MOTORS_FORCES_COLOR, UAV_FRAME_ID));
100 
101  for(size_t motorIdx = 0; motorIdx < 5; motorIdx++){
102  motorsForcesPub[motorIdx].publish(makeArrow(forces.motors[motorIdx] / 10,
103  MOTORS_FORCES_COLOR,
104  MOTOR_NAMES[motorIdx].c_str()));
105  }
106 
107  totalForcePub.publish(makeArrow(forces.total, Eigen::Vector3d(0.0, 1.0, 1.0), UAV_FRAME_ID));
108 
109  auto velocity = dynamic_cast<VtolDynamics*>(uavDynamicsSim.get())->getBodyLinearVelocity();
110  velocityPub.publish(makeArrow(velocity, SPEED_COLOR, UAV_FRAME_ID));
111 
112  liftForcePub.publish(makeArrow(forces.lift / 10, LIFT_FORCE, UAV_FRAME_ID));
113  drugForcePub.publish(makeArrow(forces.drug / 10, DRAG_FORCE, UAV_FRAME_ID));
114  sideForcePub.publish(makeArrow(forces.side / 10, SIDE_FORCE, UAV_FRAME_ID));
115 }
116 
117 visualization_msgs::Marker& RvizVisualizator::makeArrow(const Eigen::Vector3d& vector3D,
118  const Eigen::Vector3d& rgbColor,
119  const char*){
120  auto fluVector = Converter::frdToFlu(vector3D);
121  arrowMarkers.header.frame_id = UAV_FRAME_ID;
122  arrowMarkers.points[1].x = fluVector[0];
123  arrowMarkers.points[1].y = fluVector[1];
124  arrowMarkers.points[1].z = fluVector[2];
125  arrowMarkers.color.r = static_cast<float>(rgbColor[0]);
126  arrowMarkers.color.g = static_cast<float>(rgbColor[1]);
127  arrowMarkers.color.b = static_cast<float>(rgbColor[2]);
128  return arrowMarkers;
129 }
130 
131 void RvizVisualizator::initMarkers(){
132  arrowMarkers.id = 0;
133  arrowMarkers.type = visualization_msgs::Marker::ARROW;
134  arrowMarkers.action = visualization_msgs::Marker::ADD;
135  arrowMarkers.pose.orientation.w = 1;
136  arrowMarkers.scale.x = 0.05; // radius of cylinder
137  arrowMarkers.scale.y = 0.1;
138  arrowMarkers.scale.z = 0.03; // scale of hat
139  arrowMarkers.lifetime = ros::Duration();
140  arrowMarkers.color.a = 1.0;
141 
142  geometry_msgs::Point startPoint;
143  startPoint.x = 0;
144  startPoint.y = 0;
145  startPoint.z = 0;
146  arrowMarkers.points.push_back(startPoint);
147 
148  geometry_msgs::Point endPoint;
149  endPoint.x = 0;
150  endPoint.y = 0;
151  endPoint.z = 0;
152  arrowMarkers.points.push_back(endPoint);
153 }
154 
158 void RvizVisualizator::publishTf(uint8_t dynamicsNotation){
159  geometry_msgs::TransformStamped transform;
160  transform.header.stamp = ros::Time::now();
161  transform.header.frame_id = GLOBAL_FRAME_ID;
162 
163  auto position = uavDynamicsSim->getVehiclePosition();
164  auto attitude = uavDynamicsSim->getVehicleAttitude();
165  Eigen::Vector3d enuPosition;
166  Eigen::Quaterniond fluAttitude;
167  if(dynamicsNotation == PX4_NED_FRD){
168  enuPosition = Converter::nedToEnu(position);
169  fluAttitude = Converter::frdNedTofluEnu(attitude);
170  }else{
171  enuPosition = position;
172  fluAttitude = attitude;
173  }
174 
175  transform.transform.translation.x = enuPosition[0];
176  transform.transform.translation.y = enuPosition[1];
177  transform.transform.translation.z = enuPosition[2];
178  transform.transform.rotation.x = fluAttitude.x();
179  transform.transform.rotation.y = fluAttitude.y();
180  transform.transform.rotation.z = fluAttitude.z();
181  transform.transform.rotation.w = fluAttitude.w();
182  transform.child_frame_id = UAV_FRAME_ID;
183  tfPub.sendTransform(transform);
184 
185  transform.transform.rotation.x = 0;
186  transform.transform.rotation.y = 0;
187  transform.transform.rotation.z = 0;
188  transform.transform.rotation.w = 1;
189  transform.child_frame_id = UAV_FIXED_FRAME_ID;
190  tfPub.sendTransform(transform);
191 }
GLOBAL_FRAME_ID
static const char GLOBAL_FRAME_ID[]
Definition: rviz_visualization.cpp:27
VtolDynamics
Vtol dynamics simulator class.
Definition: vtolDynamicsSim.hpp:147
RvizVisualizator::motorsForcesPub
std::array< ros::Publisher, 5 > motorsForcesPub
Definition: rviz_visualization.hpp:51
RvizVisualizator::velocityPub
ros::Publisher velocityPub
Definition: rviz_visualization.hpp:61
RvizVisualizator::totalMomentPub
ros::Publisher totalMomentPub
Definition: rviz_visualization.hpp:56
RvizVisualizator::aeroMomentPub
ros::Publisher aeroMomentPub
Definition: rviz_visualization.hpp:57
MOTOR_NAMES
static const std::array< std::string, 5 > MOTOR_NAMES
Definition: rviz_visualization.cpp:26
RvizVisualizator::arrowMarkers
visualization_msgs::Marker arrowMarkers
Definition: rviz_visualization.hpp:47
RvizVisualizator::initMarkers
void initMarkers()
Definition: rviz_visualization.cpp:131
PX4_NED_FRD
static const constexpr uint8_t PX4_NED_FRD
Definition: rviz_visualization.cpp:23
RvizVisualizator::drugForcePub
ros::Publisher drugForcePub
Definition: rviz_visualization.hpp:53
UAV_FRAME_ID
static const char UAV_FRAME_ID[]
Definition: rviz_visualization.cpp:28
RvizVisualizator::publish
void publish(uint8_t dynamicsNotation)
Definition: rviz_visualization.cpp:69
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ROS_ENU_FLU
static const constexpr uint8_t ROS_ENU_FLU
Definition: rviz_visualization.cpp:24
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
RvizVisualizator::makeArrow
visualization_msgs::Marker & makeArrow(const Eigen::Vector3d &vector3D, const Eigen::Vector3d &rgbColor, const char *frameId)
Definition: rviz_visualization.cpp:117
UAV_FIXED_FRAME_ID
static const char UAV_FIXED_FRAME_ID[]
Definition: rviz_visualization.cpp:29
DynamicsNotation_t::PX4_NED_FRD
@ PX4_NED_FRD
vtolDynamicsSim.hpp
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
Converter::frdNedTofluEnu
Eigen::Quaterniond frdNedTofluEnu(const Eigen::Quaterniond &q_frd_to_ned)
Definition: cs_converter.cpp:65
RvizVisualizator::node
ros::NodeHandle & node
Definition: rviz_visualization.hpp:44
RvizVisualizator::totalForcePub
ros::Publisher totalForcePub
Definition: rviz_visualization.hpp:49
Converter::frdToFlu
Eigen::Vector3d frdToFlu(Eigen::Vector3d frd)
Definition: cs_converter.cpp:57
RvizVisualizator::aoaMomentPub
ros::Publisher aoaMomentPub
Definition: rviz_visualization.hpp:59
RvizVisualizator::uavDynamicsSim
std::shared_ptr< UavDynamicsSimBase > uavDynamicsSim
Definition: rviz_visualization.hpp:45
RvizVisualizator::init
int8_t init(const std::shared_ptr< UavDynamicsSimBase > &uavDynamicsSim_)
Definition: rviz_visualization.cpp:34
RvizVisualizator::aeroForcePub
ros::Publisher aeroForcePub
Definition: rviz_visualization.hpp:50
ros::Time
RvizVisualizator::tfPub
tf2_ros::TransformBroadcaster tfPub
Definition: rviz_visualization.hpp:63
RvizVisualizator::RvizVisualizator
RvizVisualizator(ros::NodeHandle &nh)
Definition: rviz_visualization.cpp:31
RvizVisualizator::sideForcePub
ros::Publisher sideForcePub
Definition: rviz_visualization.hpp:54
RvizVisualizator::liftForcePub
ros::Publisher liftForcePub
Definition: rviz_visualization.hpp:52
RvizVisualizator::controlSurfacesMomentPub
ros::Publisher controlSurfacesMomentPub
Definition: rviz_visualization.hpp:58
Converter::nedToEnu
Eigen::Vector3d nedToEnu(Eigen::Vector3d ned)
Definition: cs_converter.cpp:50
RvizVisualizator::motorsMomentsPub
std::array< ros::Publisher, 5 > motorsMomentsPub
Definition: rviz_visualization.hpp:60
ros::Duration
ros::NodeHandle
cs_converter.hpp
RvizVisualizator::publishTf
void publishTf(uint8_t dynamicsNotation)
Perform TF transform between GLOBAL_FRAME -> UAV_FRAME in ROS (enu/flu) format.
Definition: rviz_visualization.cpp:158
ros::Time::now
static Time now()
rviz_visualization.hpp

inno_vtol_dynamics
Author(s): Roman Fedorenko, Dmitry Ponomarev, Ezra Tal, Winter Guerra
autogenerated on Mon Dec 9 2024 03:13:35