Mocap_optitrack and mavros, no local_position/pose

asked 2019-07-08 13:57:55 -0600

GabrielBagon44 gravatar image

Hi, I am running a motion capture node in order to obtain pose data for a quadcopter. The quadcopter is using mavros and px4. The onboard computer is running ubuntu 14.04 ros indigo, while the ground computer is running ubuntu 16.04 ros kinetic.

The motion capture system publishes to mocap_node/Robot_1/pose, then the relay node subscribes to that topic and publishes it to the mavros vision_pose topic. finally from that, mavros should take that topic and use it for the /mavros/local_position/pose.

The problem is that I can rostopic echo up until the /mavros/local_position/pose topic. Once I try to echo that local_position topic I don't get any output. I am stumped as to why this might be an issue, any help will be appreciated thanks.

rqt_graph

image description

mocap relay node

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

ros::Publisher mocap_pose;
geometry_msgs::PoseStamped relay_pose;
bool relay =true;

void relay_func(const geometry_msgs::PoseStamped msg){
relay_pose=msg;
ROS_INFO_STREAM("RELAY");
if (relay==true){
ROS_INFO_STREAM("POST");
relay_pose.header.frame_id="fcu";
mocap_pose.publish(relay_pose);
relay=false;
}else{
relay=true;
}

}

int main(int argc, char **argv)
{

    //initialize ros
    ros::init(argc, argv, "mocap_relay");
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("mocap_node/Robot_1/pose", 2, relay_func);
    mocap_pose=n.advertise<geometry_msgs::PoseStamped>("mavros/vision_pose/pose",2);

ROS_INFO_STREAM("started");

    ros::spin();
}
edit retag flag offensive close merge delete