CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

simple_offboard: add position setpoint frame

Browse Source
This commit is contained in:
Oleg Kalachev
2020-03-10 19:23:40 +03:00
parent 2c36fdb560
commit 8dfb9a8f6c
3 changed files with 17 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
clover/src/simple_offboard.cpp
View File

@@ -90,7 +90,7 @@ PositionTarget position_raw_msg;
AttitudeTarget att_raw_msg;
Thrust thrust_msg;
TwistStamped rates_msg;
TransformStamped target;
TransformStamped target, setpoint;
geometry_msgs::TransformStamped body;
// State
@@ -433,6 +433,17 @@ void publish(const ros::Time stamp)
position_raw_msg.position = position_msg.pose.position;
position_raw_pub.publish(position_raw_msg);
}
// publish setpoint frame
if (!setpoint.child_frame_id.empty()) {
setpoint.transform.translation.x = position_msg.pose.position.x;
setpoint.transform.translation.y = position_msg.pose.position.y;
setpoint.transform.translation.z = position_msg.pose.position.z;
setpoint.transform.rotation = position_msg.pose.orientation;
setpoint.header.frame_id = position_msg.header.frame_id;
setpoint.header.stamp = position_msg.header.stamp;
transform_broadcaster->sendTransform(setpoint);
}
}
if (setpoint_type == VELOCITY) {
@@ -764,6 +775,7 @@ int main(int argc, char **argv)
nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
nh_priv.param("setpoint", setpoint.child_frame_id, string("setpoint"));
nh_priv.param("auto_release", auto_release, true);
nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
nh_priv.param("nav_from_sp", nav_from_sp, true);

3
docs/en/frames.md
View File

@@ -10,7 +10,8 @@ Main frames in the `clever` package:
* `map` has its origin at the flight controller initialization point and may be considered stationary. It is shown as a white grid on the image above;
* `base_link` is rigidly bound to the drone. It is shown by the simplified drone model on the image above;
* `body` is bound to the drone, but its Z axis points up regardless of the drone's pitch and roll. It is shown by the red, blue and green lines in the illustration;
* `navigate_target` is bound to the current navigation target (as set by the [navigate](simple_offboard.md#navigate) service).
* `navigate_target` is bound to the current navigation target (as set by the [navigate](simple_offboard.md#navigate) service);
* `setpoint` is current position setpoint.
Additional frames become available when [ArUco positioning system](aruco.md) is active:

3
docs/ru/frames.md
View File

@@ -10,7 +10,8 @@
* `map` — координаты относительно точки инициализации полетного контроллера: белая сетка на иллюстрации;
* `base_link` — координаты относительно квадрокоптера: схематичное изображение квадрокоптера на иллюстрации;
* `body` — координаты относительно квадрокоптера без учета наклонов по тангажу и крену: красная, синяя и зеленая линии на иллюстрации;
* `navigate_target` – координаты точки, в которую сейчас летит дрон (с использованием [navigate](simple_offboard.md#navigate)).
* `navigate_target` – координаты точки, в которую сейчас летит дрон (с использованием [navigate](simple_offboard.md#navigate));
* `setpoint` – текущий setpoint по позиции.
При использовании [системы позиционирования по ArUco-маркерам](aruco.md) появляются дополнительные фреймы: