Mavros Waypoint Data send to Pixhawk
I'm using ubuntu 16.04, ROS Kinetic, Pixhawk with latest PX4 firmware.
I want to send waypoint data to pixhawk from my ros package,
but when I launch the package, it works, but the waypoint data doesn't change.
What should I do? Here's my package.cpp code:
#include "ros/ros.h"
#include "std_msgs/String.h" #include "mavros_msgs/WaypointList.h" #include "mavros_msgs/WaypointPush.h" #include "mavros_msgs/WaypointPull.h" #include "mavros_msgs/WaypointSetCurrent.h" #include "mavros_msgs/CommandCode.h"
int main(int argc, char** argv) { ros::init(argc, argv, "pub_waypoints"); ros::NodeHandle nh;
ros::Publisher chatter_pub = nh.advertise<mavros_msgs::Waypoint>("/mavros/mission/waypoints", 100);
ros::Rate loop_rate(20);
mavros_msgs::Waypoint wp;
mavros_msgs::WaypointPush srv;
while(ros::ok())
{
wp.frame = mavros_msgs::Waypoint::FRAME_GLOBAL;
wp.command = mavros_msgs::CommandCode::NAV_WAYPOINT;
wp.is_current = true;
wp.autocontinue = true;
wp.x_lat = 20;
wp.y_long = 30;
wp.z_alt = 4;
// srv.request.waypoints.push_back(wp); chatter_pub.publish(wp);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
Plz, help me. Any reply will really help me.