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 vooon341@gmail.com * @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.
  • https://github.com/mavlink/mavros/tre... */

#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.time_boot_ms,
            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));
    transform.setRotation(uas->get_attitude_orientation());

    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)
        tf_broadcaster.sendTransform(
                tf::StampedTransform(
                    transform,
                    pose->header.stamp,
                    frame_id, child_frame_id));

    local_position.publish(pose);
}


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

Comments

/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 imageroyalcheese ( 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 imageroyalcheese ( 2015-04-29 07:02:26 -0500 )edit