How to get hector_quadrotor to fly autonomously
So, still new to ROS and the debugging of the programs. I'm trying to make a program where hector_quadrotor flies up into the air, hovers, spins, and then continues to hover before going to the next task. I've gotten it to work with just forcing the functions through:
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Imu.h>
// tf Subscriber
#include <tf/tfMessage.h>
#include <tf/transform_listener.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
// IOStream for Testing
#include <iostream>
namespace quad_lift_spin {// Structure of Elements
// initialization of global varibles
int mapYSize = 0, mapXSize = 0, numAgents, agentID;
float agentLocation[3] = { 0, 0, 0 };
geometry_msgs::Twist lift_msg;
}
using namespace quad_lift_spin;
void quad_lift()
{
lift_msg.linear.x = 0;
lift_msg.linear.y = 0;
lift_msg.linear.z = 0;
lift_msg.angular.x = 0;
lift_msg.angular.y = 0;
lift_msg.angular.z = 0;
lift_msg.linear.z = 0.5;
ros::Duration(2).sleep(); //wait 2 seconds
//quadLSPub.publish(lift_msg);
/*lift_msg.linear.z = 0;
ros::Duration(4).sleep(); //wait 4 seconds
//quadLSPub.publish(lift_msg);*/
}
void quad_hover()
{
lift_msg.linear.x = 0;
lift_msg.linear.y = 0;
lift_msg.linear.z = 0;
lift_msg.angular.x = 0;
lift_msg.angular.y = 0;
lift_msg.angular.z = 0;
/*lift_msg.linear.z = 0.5;
ros::Duration(2).sleep(); //wait 2 seconds
//quadLSPub.publish(lift_msg);*/
//lift_msg.linear.z = 0;
//ros::Duration(4).sleep(); //wait 4 seconds
//quadLSPub.publish(lift_msg);
}
void quad_spin()
{
lift_msg.linear.x = 0;
lift_msg.linear.y = 0;
lift_msg.linear.z = 0;
lift_msg.angular.x = 0;
lift_msg.angular.y = 0;
lift_msg.angular.z = 0;
lift_msg.angular.z = 0.5;
ros::Duration(2).sleep(); //wait 2 seconds
//quadLSPub.publish(lift_msg);
/*lift_msg.angular.z = 0;
ros::Duration(4).sleep(); //wait 4 seconds
//quadLSPub.publish(lift_msg);*/
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "quad_lift_spin");
ros::NodeHandle nh;
//ros::NodeHandle pnh("~");
ros::Publisher quadLSPub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
quad_lift();
quadLSPub.publish(lift_msg);
ros::Duration(2).sleep();
quad_hover();
quadLSPub.publish(lift_msg);
ros::Duration(3).sleep();
quad_spin();
quadLSPub.publish(lift_msg);
ros::Duration(5).sleep();
quad_hover();
quadLSPub.publish(lift_msg);
}
With this working, I've been trying to figure out the publishers and subscribers needed to get the separate functions to work and have run into a snag with the publishing and subscribing:
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Header.h>
#include "target_alert/alert.h"
// tf Subscriber
#include <tf/tfMessage.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
// IOStream for Testing
#include <iostream>
namespace quad_lift_spin {// Structure of Elements
// initialization of global varibles
int mapYSize = 0, mapXSize = 0, numAgents, agentID;
float agentLocation[3] = { 0, 0, 0 };
geometry_msgs::Twist lift_msg;
sensor_msgs::Range sonar_height;
int flight_mode;
}
using namespace quad_lift_spin;
void quad_lift(const sensor_msgs::Range::ConstPtr &msg)
{
lift_msg.linear.x = 0;
lift_msg.linear.y = 0;
lift_msg.linear.z = 0;
lift_msg.angular.x = 0;
lift_msg.angular.y = 0;
lift_msg.angular.z = 0;
if(flight_mode == 0)
{
std::cout<< "went into if zero" << std::endl;
lift_msg.linear.z = 0.5;
double pastTime = msg-> header.stamp.nsec;
if (msg->range >=1.2 && msg->range <=1.6)
{
double nowTime = msg-> header.stamp.nsec;
double overalTime = nowTime-pastTime;
if (overalTime > 1500000000)
{
lift_msg.linear.z = 0;
flight_mode == 1;
}
}
}
if(flight_mode == 1)
{
double pastTime = msg-> header.stamp.nsec;
if (msg->range >=1.2 && msg->range <=1.6)
{
double nowTime = msg-> header.stamp.nsec;
double overalTime = nowTime-pastTime;
if (overalTime > 1500000000)
{
lift_msg.linear.z = 0;
}
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "quad_lift_spin");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
static std::string flyTopic;
if (!pnh.getParam("flyTopic", flyTopic))
{
flyTopic = "/cmd_vel";
}
ros::Subscriber quadSub = nh.subscribe<sensor_msgs::Range>(flyTopic, 10, quad_lift);
ros::Publisher quadLSPub = nh.advertise<geometry_msgs::Twist>("move_base", 10);
flight_mode = 0;
std::cout<< flight_mode << std::endl;
while (ros::ok())
{
//std::cout<< lift_msg << std::endl;
quadLSPub.publish(lift_msg);
}
}
I am really feeling stuck and not sure where else to go for references. If anybody can correct the publisher/subscriber or can give a link to help me out the hector_quadrotor, that would be fantastic! Please and thank you!
Asked by jessiems10 on 2015-08-24 15:39:19 UTC
Answers
I was able to find two things after trial and error with a coworker. The flyTopic
needed to be with "sonar_height"
which connects with the range or height off the ground, and the geometry_msgs:Twist
will still need to "cmd_vel"
instead of "move_base"
.
The other thing (which made me feel really silly) was that you need ros::spingOnce();
in the while(ros::ok())
, otherwise the program won't get any of the callbacks.
Now I'm having issues with the time portion so I've started debugging that. If anyone else sees anything, please let me know!
Asked by jessiems10 on 2015-08-25 12:41:53 UTC
Comments
Also, using ROS Indigo and Ubuntu 14.04 lts
Asked by jessiems10 on 2015-08-25 10:30:34 UTC