Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[WIP] Plugins: setpoint_attitude: add sync between thrust and attitude #700

Merged
merged 12 commits into from
Jun 28, 2017
Merged

[WIP] Plugins: setpoint_attitude: add sync between thrust and attitude #700

Show file tree
Hide file tree
Changes from 11 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
1f01ab8
plugins: setpoint_attitude: add sync between throttle and attitude to…
TSC21 May 2, 2017
f84b046
plugins: typo correction: replace throttle with thrust
TSC21 May 2, 2017
175479e
plugins: msgs: setpoint_attitude: replaces Float32Stamped for Thrust msg
TSC21 May 2, 2017
35d31e3
plugins: setpoint_attitude: add sync between twist and thrust (RPY+Th…
TSC21 May 2, 2017
008c99e
setpoint_attitude: update the logic of thrust normalization verification
TSC21 Jun 10, 2017
4b45304
setpoint_attitude: implement sync between tf listener and thrust subs…
TSC21 Jun 11, 2017
b958de3
TF sync listener: generalize topic type that can be syncronized with TF2
TSC21 Jun 15, 2017
98e6535
TF2ListenerMixin: keep class template, use template for tf sync metho…
TSC21 Jun 22, 2017
f50453c
TF2ListenerMixin: fix and improve sync tf2_start method
TSC21 Jun 24, 2017
2927dc9
general update to yaml config files and parameters
TSC21 Jun 24, 2017
c4d9757
setpoint_attitude: add note on Thrust sub name
TSC21 Jun 28, 2017
7fd9231
setpoint_attitude: TF sync: pass subscriber pointer instead of bindin…
TSC21 Jun 28, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 46 additions & 1 deletion mavros/include/mavros/setpoint_mixin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,10 @@

#include <geometry_msgs/TransformStamped.h>

#include "tf2_ros/message_filter.h"
#include <message_filters/subscriber.h>


namespace mavros {
namespace plugin {
/**
Expand Down Expand Up @@ -122,7 +126,7 @@ class SetPositionTargetGlobalIntMixin {
template <class D>
class SetAttitudeTargetMixin {
public:
//! Message sepecification: @p http://mavlink.org/messages/common#SET_ATTITIDE_TARGET
//! Message sepecification: @p http://mavlink.org/messages/common#SET_ATTITUDE_TARGET
void set_attitude_target(uint32_t time_boot_ms,
uint8_t type_mask,
Eigen::Quaterniond orientation,
Expand Down Expand Up @@ -200,6 +204,47 @@ class TF2ListenerMixin {
}
});
}

/**
* @brief start tf listener syncronized with another topic
*
* @param _thd_name listener thread name
* @param cbp plugin callback function
*/
template <class T>
void tf2_start(const char *_thd_name, void (D::*cbp)(const geometry_msgs::TransformStamped &, const typename T::ConstPtr &))
{
tf_thd_name = _thd_name;

tf_thread = std::thread([this, cbp]() {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more. 

void tf2_start(const char *_thd_name, message_fileters::Subscriber<T> &tsub, ...)
...
th_therad = std::thread([this, cbp, tsub]() {
...

mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str());

mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
ros::NodeHandle &_sp_nh = static_cast<D *>(this)->sp_nh;
std::string &_frame_id = static_cast<D *>(this)->tf_frame_id;
std::string &_child_frame_id = static_cast<D *>(this)->tf_child_frame_id;

message_filters::Subscriber<T> &_topic_sub = static_cast<D *>(this)->sub;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Pass it via args!

tf2_ros::MessageFilter<T> tf2_filter(_topic_sub, m_uas_->tf2_buffer, _frame_id, 10, _sp_nh);

ros::Rate rate(static_cast<D *>(this)->tf_rate);
while (ros::ok()) {
// Wait up to 3s for transform
if (m_uas_->tf2_buffer.canTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0))) {
try {
auto transform = m_uas_->tf2_buffer.lookupTransform(
_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0));

tf2_filter.registerCallback(boost::bind(cbp, static_cast<D *>(this), transform, _1));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That looks weird. Creating new filter on each TF rx not good. And _topic_sub not initialized, there no topic name.
Do you test that?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I had _top_sub initialized before in a proper way. I don't know why I changed it. I would roll back and initialize it on plugin and use it here on the template by applying static_cast<D *>(this)->. Sorry for that

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Creating new filter on each TF rx not good.

Yeah I will put it outside the while loop.

}
catch (tf2::LookupException &ex) {
ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what());
}
}
rate.sleep();
}
});
}
};
} // namespace plugin
} // namespace mavros
20 changes: 12 additions & 8 deletions mavros/launch/apm_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,19 +85,20 @@ setpoint_accel:

# setpoint_attitude
setpoint_attitude:
reverse_throttle: false # allow reversed throttle
reverse_thrust: false # allow reversed thrust
use_quaternion: false # enable PoseStamped topic subscriber
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "local_origin"
child_frame_id: "attitude"
rate_limit: 10.0
frame_id: "map"
child_frame_id: "target_attitude"
rate_limit: 50.0

# setpoint_position
setpoint_position:
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "local_origin"
child_frame_id: "setpoint"
frame_id: "map"
child_frame_id: "target_position"
rate_limit: 50.0

# setpoint_velocity
Expand Down Expand Up @@ -151,8 +152,8 @@ px4flow:
vision_pose:
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "local_origin"
child_frame_id: "vision"
frame_id: "map"
child_frame_id: "vision_estimate"
rate_limit: 10.0

# vision_speed_estimate
Expand All @@ -163,4 +164,7 @@ vision_speed:
vibration:
frame_id: "vibration"

# adsb
# None

# vim:set ts=2 sw=2 et:
17 changes: 9 additions & 8 deletions mavros/launch/px4_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,19 +86,20 @@ setpoint_accel:

# setpoint_attitude
setpoint_attitude:
reverse_throttle: false # allow reversed throttle
reverse_thrust: false # allow reversed thrust
use_quaternion: false # enable PoseStamped topic subscriber
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "local_origin"
child_frame_id: "attitude"
rate_limit: 10.0
frame_id: "map"
child_frame_id: "target_attitude"
rate_limit: 50.0

# setpoint_position
setpoint_position:
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "local_origin"
child_frame_id: "setpoint"
frame_id: "map"
child_frame_id: "target_position"
rate_limit: 50.0

# setpoint_velocity
Expand Down Expand Up @@ -163,8 +164,8 @@ px4flow:
vision_pose:
tf:
listen: false # enable tf listener (disable topic subscribers)
frame_id: "local_origin"
child_frame_id: "vision"
frame_id: "map"
child_frame_id: "vision_estimate"
rate_limit: 10.0

# vision_speed_estimate
Expand Down
12 changes: 6 additions & 6 deletions mavros/src/plugins/setpoint_accel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,12 +81,12 @@ class SetpointAccelerationPlugin : public plugin::PluginBase,
auto accel = ftf::transform_frame_enu_ned(accel_enu);

set_position_target_local_ned(stamp.toNSec() / 1000000,
utils::enum_value(MAV_FRAME::LOCAL_NED),
ignore_all_except_a_xyz,
Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero(),
accel,
0.0, 0.0);
utils::enum_value(MAV_FRAME::LOCAL_NED),
Copy link
Member Author

@TSC21 TSC21 May 2, 2017 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I applied the uncrustify to the wrong file initially so that's why this changed.

ignore_all_except_a_xyz,
Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero(),
accel,
0.0, 0.0);
}

/* -*- callbacks -*- */
Expand Down
Loading