Problem with callback function, dynamically change buffer

asked 2015-04-29 06:25:55 -0500

royalcheese gravatar image

Hi everyone ! Here is my code`/** * @brief LocalPosition plugin * @file local_position.cpp * @author Vladimir Ermakov * @author Glenn Gregory

  • @addtogroup plugin
  • @{ / /
  • Copyright 2014 Vladimir Ermakov. *@add by clement perrousset
  • This file is part of the mavros package and subject to the license terms
  • in the top-level LICENSE file of the mavros repository.
  • */

#include <mavros mavros_plugin.h=""> #include <pluginlib class_list_macros.h="">

#include <tf transform_broadcaster.h=""> #include <geometry_msgs posestamped.h="">

namespace mavplugin { /** * @brief Local position plugin. * Publish local position to TF and PositionStamped, */ class LocalPositionPlugin : public MavRosPlugin { public: LocalPositionPlugin() : lp_nh("~local_position"), uas(nullptr), send_tf(false) { };

void initialize(UAS &uas_)
    uas = &uas_;

    lp_nh.param("send_tf", send_tf, true);
    lp_nh.param<std::string>("frame_id", frame_id, "local_origin");
    lp_nh.param<std::string>("child_frame_id", child_frame_id, "fcu");

    local_position = lp_nh.advertise<geometry_msgs::PoseStamped>("local", 10);
    //my_local_pos =lp_nh.subscribe("my_pos", 50,myposcallback);

const message_map get_rx_handlers() {
    return {
               MESSAGE_HANDLER(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &LocalPositionPlugin::handle_local_position_ned)

private: ros::NodeHandle lp_nh; UAS *uas;

geometry_msgs::PoseStamped my_pos_buff ;
ros::Subscriber my_local_pos ;
ros::Publisher local_position;
tf::TransformBroadcaster tf_broadcaster;

std::string frame_id;       //!< origin for TF
std::string child_frame_id; //!< frame for TF and Pose
bool send_tf;

void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
    mavlink_local_position_ned_t pos_ned;
    mavlink_msg_local_position_ned_decode(msg, &pos_ned);

    ROS_DEBUG_THROTTLE_NAMED(10, "position", "Local position NED: boot_ms:%06d "
            "position:(%1.3f %1.3f %1.3f) speed:(%1.3f %1.3f %1.3f)",
            pos_ned.x, pos_ned.y, pos_ned.z,
            pos_ned.vx, pos_ned.vy, pos_ned.vz);

    /* TODO: check convertion to ENU
     * I think XZY is not body-fixed, but orientation does.
     * Perhaps this adds additional errorprone to us.
     * Need more tests. Issue #49.
     * orientation in ENU, body-fixed
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(pos_ned.y, pos_ned.x, -pos_ned.z));

    auto pose = boost::make_shared<geometry_msgs::PoseStamped>();

    tf::poseTFToMsg(transform, pose->pose);
    pose->header.frame_id = frame_id;
    pose->header.stamp = uas->synchronise_stamp(pos_ned.time_boot_ms);

    if (send_tf)
                    frame_id, child_frame_id));


void myposcallback (const geometry_msgs::PoseStamped::ConstPtr &req,geometry_msgs::PoseStamped my_pos_buff) 
my_pos_buff.pose =req->pose ;
my_pos_buff.header= req->header;

}; }; // namespace mavplugin

PLUGINLIB_EXPORT_CLASS(mavplugin::LocalPositionPlugin, mavplugin::MavRosPlugin) `

I would listen a geometry_msg by myposcallback through lp_nh.subscribe in order to fill "my_pos_buff" and to ultimately send the data in local_position.

for context i'm trying to shortcut the local_position data to send my own.

I'm having problem when i use //my_local_pos =lp_nh.subscribe("my_pos", 50,myposcallback) , and i'm having problem if i want to use this.my_pos_buffer in callback or in lp_nh.subscribe.

Any idea to help me ? Lots of thank in advance, I do not understand where I'm mistaken . have a good day

edit retag flag offensive close merge delete


/home/clementperrousset/catkin_ws/src/mavros/mavros/src/plugins/local_position.cpp:46:78: error: no matching function for call to ‘ros::NodeHandle::subscribe(const char [7], int, <unresolved overloaded="" function="" type="">, geometry_

royalcheese gravatar image royalcheese  ( 2015-04-29 07:01:12 -0500 )edit

have tried my_local_pos =lp_nh.subscribe("my_pos", 50,myposcallback,this->my_pos_buff); lp_nh.subscribe("my_pos", 50,myposcallback,this-); lp_nh.subscribe("my_pos", 50,myposcallback,my_pos_buff); already...

royalcheese gravatar image royalcheese  ( 2015-04-29 07:02:26 -0500 )edit