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

Add one auxiliary frames node, rename ‘fcu_horiz’ to ‘body’ by default

Browse Source
This commit is contained in:
Oleg Kalachev
2018-09-28 03:39:28 +03:00
parent b03919ed86
commit a1f29738ab
3 changed files with 70 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
clever/CMakeLists.txt
View File

@@ -165,10 +165,14 @@ add_executable(rc src/rc.cpp)
add_executable(camera_markers src/camera_markers.cpp)
add_executable(frames src/frames.cpp)
target_link_libraries(rc ${catkin_LIBRARIES})
target_link_libraries(camera_markers ${catkin_LIBRARIES})
target_link_libraries(frames ${catkin_LIBRARIES})
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use

5
clever/launch/clever.launch
View File

@@ -43,11 +43,12 @@
<node pkg="tf2_ros" type="static_transform_publisher" name="local_origin_upside_down_frame" args="0 0 0 3.1415926 3.1415926 0 local_origin local_origin_upside_down"/>
<!-- fcu_horiz frame -->
<node pkg="nodelet" type="nodelet" name="fcu_horiz" args="standalone clever/fcu_horiz" output="screen" clear_params="true"/>
<!-- simplified offboard control -->
<node name="simple_offboard" pkg="clever" type="simple_offboard.py" output="screen"/>
<!-- Auxiliary frames -->
<node name="frames" pkg="clever" type="frames" output="screen"/>
<!-- main camera -->
<include file="$(find clever)/launch/main_camera.launch" if="$(arg main_camera)"/>

63
clever/src/frames.cpp Normal file
View File

@@ -0,0 +1,63 @@
/*
* Auxiliary TF frames for CLEVER drone kit:
* - Body frame (drone body with zero pitch and roll).
* - TODO: REP-0105 `odom` frame emulation: continuous frame without discrete jumps.
* - TODO: Terrain frame (base on ALTITUDE message).
* - TODO: map_upside_down frame
* - TODO: home frame?
*
* Copyright (C) 2018 Copter Express Technologies
*
* Author: Oleg Kalachev <okalachev@gmail.com>
*
* Distributed under MIT License (available at https://opensource.org/licenses/MIT).
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*/
// TODO: consider implementing as a mavros plugin
#include <string>
#include <memory>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
using std::string;
static std::shared_ptr<tf2_ros::TransformBroadcaster> br;
static geometry_msgs::TransformStamped body;
inline void publishBody(const geometry_msgs::PoseStamped& pose)
{
// Get only yaw from pose
tf::Quaternion q;
q.setRPY(0, 0, tf::getYaw(pose.pose.orientation));
tf::quaternionTFToMsg(q, body.transform.rotation);
body.transform.translation.x = pose.pose.position.x;
body.transform.translation.y = pose.pose.position.y;
body.transform.translation.z = pose.pose.position.z;
body.header.frame_id = pose.header.frame_id;
body.header.stamp = pose.header.stamp;
br->sendTransform(body);
}
void poseCallback(const geometry_msgs::PoseStamped& pose)
{
publishBody(pose);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "frames");
ros::NodeHandle nh, nh_priv("~");
nh_priv.param<string>("body/frame_id", body.child_frame_id, "body");
br = std::make_shared<tf2_ros::TransformBroadcaster>();
ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &poseCallback);
ROS_INFO("frames: ready");
ros::spin();
}