bool KIScaleInit::update(const monoslam_ros_test::filter_stateConstPtr statePtr)
{
if (_numCycle == 2) {
node->_scalePub.publish(std_msgs::Empty());
return true;
}
if ( node->_droneState == HOVERING && _bNextUp) {
_startTS = node->getRelativeTime(ros::Time::now());
ROS_INFO("Ready to rise\n");
ROS_INFO("Current time: %d\n", _startTS);
_bNextUp = false;
node->sendControlToDrone(ControlCommand(0,0,0,0.4*_controlGain));
_bRising = true;
return false;
}
else if (_bRising)
{
if( (node->getRelativeTime(ros::Time::now()) - _startTS) < 1500)
node->sendControlToDrone(ControlCommand(0,0,0,0.4*_controlGain));
else {
node->sendControlToDrone(node->hoverCommand);
_bRising = false;
_bNextDown = true;
_numCycle++;
}
return false;
}
else if ( node->_droneState == HOVERING && _bNextDown) {
_startTS = node->getRelativeTime(ros::Time::now());
ROS_INFO("Ready to drop\n");
ROS_INFO("Current time: %d\n", _startTS);
_bNextDown = false;
node->sendControlToDrone(ControlCommand(0,0,0,-0.4*_controlGain));
_bDropping = true;
return false;
}
else if (_bDropping) {
if( (node->getRelativeTime(ros::Time::now()) - _startTS) < 1500)
node->sendControlToDrone(ControlCommand(0,0,0,-0.4*_controlGain));
else {
node->sendControlToDrone(node->hoverCommand);
_bDropping = false;
_bNextUp = true;
}
return false;
}
return false;
}
RosThread::RosThread()
{
gui = NULL;
navdataCount = velCount = dronePoseCount = joyCount = velCount100ms = 0;
keepRunning = true;
lastJoyControlSent = ControlCommand(0,0,0,0);
lastL1Pressed = lastR1Pressed = false;
}
int main(int argc, char* argv[]){
int rosargc = 1;
ros::init(rosargc, argv, "flying_drum_machine");
ros::Time::init();
ros::Rate r(Global::rate);
Messenger* messenger = new Messenger();
DrumMachine* dm = new DrumMachine(messenger);
signal(SIGINT, [](int signum) {
std::cout << "\nDrum machine shutdown" << std::endl;
quit = true;
});
// In case we need to reset after emergency landing or crash
messenger->sendToggleState();
// Calibrate the IMU and flat trim
// ros::service::call("ardrone/flattrim");
// ros::service::call("ardrone/imu_recalib");
while(!quit){
ros::spinOnce();
dm->execute();
r.sleep();
}
// Send a hover command
messenger->sendControlToDrone(ControlCommand());
// Land
messenger->sendLand();
delete messenger;
delete dm;
return 0;
}
void RosThread::run()
{
std::cout << "Starting ROS Thread" << std::endl;
vel_pub = nh_.advertise<geometry_msgs::Twist>(nh_.resolveName("cmd_vel"),1);
vel_sub = nh_.subscribe(nh_.resolveName("cmd_vel"),50, &RosThread::velCb, this);
tum_ardrone_pub = nh_.advertise<std_msgs::String>(nh_.resolveName("tum_ardrone/com"),50);
tum_ardrone_sub = nh_.subscribe(nh_.resolveName("tum_ardrone/com"),50, &RosThread::comCb, this);
dronepose_sub = nh_.subscribe(nh_.resolveName("ardrone/predictedPose"),50, &RosThread::droneposeCb, this);
navdata_sub = nh_.subscribe(nh_.resolveName("ardrone/navdata"),50, &RosThread::navdataCb, this);
joy_sub = nh_.subscribe(nh_.resolveName("joy"),50, &RosThread::joyCb, this);
takeoff_pub = nh_.advertise<std_msgs::Empty>(nh_.resolveName("ardrone/takeoff"),1);
land_pub = nh_.advertise<std_msgs::Empty>(nh_.resolveName("ardrone/land"),1);
toggleState_pub = nh_.advertise<std_msgs::Empty>(nh_.resolveName("ardrone/reset"),1);
takeoff_sub = nh_.subscribe(nh_.resolveName("ardrone/takeoff"),1, &RosThread::takeoffCb, this);
land_sub = nh_.subscribe(nh_.resolveName("ardrone/land"),1, &RosThread::landCb, this);
toggleState_sub = nh_.subscribe(nh_.resolveName("ardrone/reset"),1, &RosThread::toggleStateCb, this);
toggleCam_srv = nh_.serviceClient<std_srvs::Empty>(nh_.resolveName("ardrone/togglecam"),1);
flattrim_srv = nh_.serviceClient<std_srvs::Empty>(nh_.resolveName("ardrone/flattrim"),1);
ros::Time last = ros::Time::now();
ros::Time lastHz = ros::Time::now();
while(keepRunning && nh_.ok())
{
// spin for 100ms
while((ros::Time::now() - last) < ros::Duration(0.1))
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1 - (ros::Time::now() - last).toSec()));
last = ros::Time::now();
// if nothing on /cmd_vel, repeat!
if(velCount100ms == 0)
switch(gui->currentControlSource)
{
case CONTROL_AUTO:
sendControlToDrone(ControlCommand(0,0,0,0));
break;
case CONTROL_JOY:
sendControlToDrone(lastJoyControlSent);
break;
case CONTROL_KB:
sendControlToDrone(gui->calcKBControl());
break;
case CONTROL_NONE:
sendControlToDrone(ControlCommand(0,0,0,0));
break;
}
velCount100ms = 0;
// if 1s passed: update Hz values
if((ros::Time::now() - lastHz) > ros::Duration(1.0))
{
gui->setCounts(navdataCount, velCount, dronePoseCount, joyCount);
navdataCount = velCount = dronePoseCount = joyCount = 0;
lastHz = ros::Time::now();
}
}
gui->closeWindow();
if(nh_.ok()) ros::shutdown();
std::cout << "Exiting ROS Thread (ros::shutdown() has been called)" << std::endl;
}
