diff --git a/asctec_autopilot/CMakeLists.txt b/asctec_autopilot/CMakeLists.txt index 895ee0d..824e6c0 100644 --- a/asctec_autopilot/CMakeLists.txt +++ b/asctec_autopilot/CMakeLists.txt @@ -23,6 +23,8 @@ rosbuild_add_library (autopilot src/autopilot.cpp src/crc16.cpp src/telemetry.cpp) +rosbuild_add_library(waypointcontroller src/waypointcontroller.cpp) + # create autopilot_nodelet library rosbuild_add_library (autopilot_nodelet src/autopilot_nodelet.cpp) @@ -32,3 +34,8 @@ target_link_libraries (autopilot_nodelet autopilot) rosbuild_add_executable(autopilot_node src/autopilot_node.cpp) target_link_libraries (autopilot_node autopilot) + +rosbuild_add_executable(waypointcontroller_node src/waypointcontroller_node.cpp) +target_link_libraries (waypointcontroller_node waypointcontroller) + +#rosbuild_add_executable(bag_to_gpscsv src/bag_to_gpscsv.cpp) diff --git a/asctec_autopilot/include/asctec_autopilot/autopilot.h b/asctec_autopilot/include/asctec_autopilot/autopilot.h index 8c8ede8..5feb62a 100644 --- a/asctec_autopilot/include/asctec_autopilot/autopilot.h +++ b/asctec_autopilot/include/asctec_autopilot/autopilot.h @@ -79,10 +79,15 @@ namespace asctec bool enable_CONTROL_; int interval_CONTROL_; int offset_CONTROL_; - + bool enable_CURRENTWAY_; + int interval_CURRENTWAY_; + int offset_CURRENTWAY_; + bool enable_WAYPOINT_; + SerialInterface* serialInterface_; Telemetry* telemetry_; + // Diagnostics diagnostic_updater::Updater diag_updater_; double last_spin_time_; diff --git a/asctec_autopilot/include/asctec_autopilot/serialinterface.h b/asctec_autopilot/include/asctec_autopilot/serialinterface.h index ab95a1a..ebf7e3b 100644 --- a/asctec_autopilot/include/asctec_autopilot/serialinterface.h +++ b/asctec_autopilot/include/asctec_autopilot/serialinterface.h @@ -49,6 +49,7 @@ namespace asctec void output (unsigned char *output, int len); bool getPackets (Telemetry *telemetry); void sendControl (Telemetry *telemetry); + void sendWaypoint (Telemetry *telemetry); void sendEstop (Telemetry *telemetry); void dumpDebug (void); bool getPacket (char *spacket, unsigned char &packet_type, unsigned short &packet_crc, unsigned short &packet_size); diff --git a/asctec_autopilot/include/asctec_autopilot/telemetry.h b/asctec_autopilot/include/asctec_autopilot/telemetry.h index 8bd3cdf..d91571c 100644 --- a/asctec_autopilot/include/asctec_autopilot/telemetry.h +++ b/asctec_autopilot/include/asctec_autopilot/telemetry.h @@ -30,7 +30,10 @@ #include "asctec_msgs/GPSData.h" #include "asctec_msgs/GPSDataAdvanced.h" #include "asctec_msgs/CtrlInput.h" +#include "asctec_msgs/CurrentWay.h" +#include "asctec_msgs/Waypoint.h" #include +#include namespace asctec { @@ -115,6 +118,8 @@ namespace asctec void publishPackets(); void enableControl (Telemetry * telemetry_, uint8_t interval = 1, uint8_t offset = 0); + void enableWaypoints (Telemetry * telemetry_); + void dumpLL_STATUS(); void dumpIMU_RAWDATA(); @@ -132,11 +137,14 @@ namespace asctec void copyCONTROLLER_OUTPUT(); void copyGPS_DATA(); void copyGPS_DATA_ADVANCED(); + void copyWAYPOINT(); void copyCTRL_INPUT(asctec_msgs::CtrlInput msg); + void copyWAYPOINT_INPUT(asctec_msgs::Waypoint msg); void estopCallback(const std_msgs::Bool msg); bool pollingEnabled_; bool controlEnabled_; + bool waypointEnabled_; uint16_t requestCount_; uint16_t controlCount_; std::bitset < 16 > requestPackets_; @@ -157,6 +165,9 @@ namespace asctec ros::Subscriber controlSubscriber_; ros::Subscriber estopSubscriber_; + ros::Subscriber waypointSubscriber_; + bool wp_received_; + //packet descriptors static const uint8_t PD_IMURAWDATA = 0x01; static const uint8_t PD_LLSTATUS = 0x02; @@ -344,6 +355,26 @@ namespace asctec int speed_y_best_estimate; }; + struct CURRENT_WAY { + unsigned char dummy1; + unsigned char properties; + unsigned short nr_of_wp; //don't care + + unsigned char current_wp; //don't care + unsigned char current_wp_memlocation; //don't care + + unsigned char status; //don't care + unsigned char dummy2; + + unsigned short navigation_status; //see WP_NAVSTAT_... defines + unsigned short distance_to_wp; //distance to WP in dm + }; + + #define WP_NAVSTAT_REACHED_POS 0x01 + #define WP_NAVSTAT_REACHED_POS_TIME 0x02 //vehicle is within a radius of WAYPOINT.pos_acc and time to stay is over + #define WP_NAVSTAT_20M 0x04 //vehicle within a 20m radius of the waypoint + #define WP_NAVSTAT_PILOT_ABORT 0x08 //waypoint navigation aborted by safety pilot + struct WAYPOINT { //always set to 1 @@ -370,6 +401,14 @@ namespace asctec //height over 0 reference in mm int height; }; + + #define WPROP_ABSCOORDS 0x01 + #define WPROP_HEIGHTENABLED 0x02 + #define WPROP_YAWENABLED 0x04 + #define WPROP_AUTOMATICGOTO 0x10 + #define WPROP_CAM_TRIGGER 0x20 + + struct CTRL_INPUT { //serial commands (= Scientific Interface) @@ -426,9 +465,10 @@ You will receive an acknowledge if a command or a waypoint was received correctl struct RC_DATA RC_DATA_; struct CONTROLLER_OUTPUT CONTROLLER_OUTPUT_; struct GPS_DATA GPS_DATA_; - struct WAYPOINT WAYPOINT_; struct GPS_DATA_ADVANCED GPS_DATA_ADVANCED_; + struct CURRENT_WAY WAYPOINT_; struct CTRL_INPUT CTRL_INPUT_; + struct WAYPOINT WAYPOINT_INPUT_; asctec_msgs::LLStatusPtr LLStatus_; asctec_msgs::IMURawDataPtr IMURawData_; asctec_msgs::IMUCalcDataPtr IMUCalcData_; @@ -436,6 +476,7 @@ You will receive an acknowledge if a command or a waypoint was received correctl asctec_msgs::ControllerOutputPtr ControllerOutput_; asctec_msgs::GPSDataPtr GPSData_; asctec_msgs::GPSDataAdvancedPtr GPSDataAdvanced_; + asctec_msgs::CurrentWayPtr CurrentWay_; ros::NodeHandle nh_; //asctec_msgs::CtrlInput CtrlInput_; diff --git a/asctec_autopilot/include/asctec_autopilot/waypointcontroller.h b/asctec_autopilot/include/asctec_autopilot/waypointcontroller.h new file mode 100644 index 0000000..106c185 --- /dev/null +++ b/asctec_autopilot/include/asctec_autopilot/waypointcontroller.h @@ -0,0 +1,57 @@ +/* + * AscTec Waypoint Controller + * Copyright (C) 2014 + * Matt Sheckells + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + + +#ifndef WAYPOINT_CONTROLLER_H_ +#define WAYPOINT_CONTROLLER_H_ + +#include +#include +#include +#include "asctec_autopilot/telemetry.h" + + +namespace asctec +{ + class WaypointController + { + public: + WaypointController(ros::NodeHandle nh, ros::NodeHandle nh_private); + void spin(const ros::TimerEvent & e); + void handleCurrentway(asctec_msgs::CurrentWay msg); + + private: + bool openWayFile(std::string filename); + bool hasNextWaypoint(); + struct Telemetry::WAYPOINT getNextWaypoint(); + asctec_msgs::Waypoint getNextMessage(); + + ros::Timer timer_; + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + + ros::Subscriber currentwaySubscriber_; + ros::Publisher waypointPublisher_; + + std::vector< struct Telemetry::WAYPOINT > wps_; + unsigned int cur_wp_; + std::string waypointFile_; + }; +} +#endif diff --git a/asctec_autopilot/launch/log_sensors.launch b/asctec_autopilot/launch/log_sensors.launch new file mode 100644 index 0000000..3d28a67 --- /dev/null +++ b/asctec_autopilot/launch/log_sensors.launch @@ -0,0 +1,30 @@ + + + #### Asctec Autopilot ################################### + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/asctec_autopilot/launch/waypoint.launch b/asctec_autopilot/launch/waypoint.launch new file mode 100644 index 0000000..2a363db --- /dev/null +++ b/asctec_autopilot/launch/waypoint.launch @@ -0,0 +1,53 @@ + + + #### Asctec Autopilot ################################### + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/asctec_autopilot/src/autopilot.cpp b/asctec_autopilot/src/autopilot.cpp index 0eff664..8b873d6 100644 --- a/asctec_autopilot/src/autopilot.cpp +++ b/asctec_autopilot/src/autopilot.cpp @@ -32,7 +32,7 @@ namespace asctec diag_updater_() { ROS_INFO ("Creating AutoPilot Interface"); - + // **** get parameters if (!nh_private_.getParam ("freq", freq_)) @@ -58,6 +58,10 @@ namespace asctec enable_GPS_DATA_ADVANCED_ = false; if (!nh_private_.getParam ("enable_CONTROL", enable_CONTROL_)) enable_CONTROL_ = false; + if (!nh_private_.getParam ("enable_WAYPOINT", enable_WAYPOINT_)) + enable_WAYPOINT_ = false; + if (!nh_private_.getParam ("enable_CURRENTWAY", enable_CURRENTWAY_)) + enable_CURRENTWAY_ = enable_WAYPOINT_; if (!nh_private_.getParam ("interval_LL_STATUS", interval_LL_STATUS_)) interval_LL_STATUS_ = 1; @@ -75,6 +79,8 @@ namespace asctec interval_GPS_DATA_ADVANCED_ = 1; if (!nh_private_.getParam ("interval_CONTROL", interval_CONTROL_)) interval_CONTROL_ = 1; + if (!nh_private_.getParam ("interval_CURRENTWAY", interval_CURRENTWAY_)) + interval_CURRENTWAY_ = 1; if (!nh_private_.getParam ("offset_LL_STATUS", offset_LL_STATUS_)) offset_LL_STATUS_ = 0; @@ -92,6 +98,8 @@ namespace asctec offset_GPS_DATA_ADVANCED_ = 0; if (!nh_private_.getParam ("offset_CONTROL", offset_CONTROL_)) offset_CONTROL_ = 0; + if (!nh_private_.getParam ("offset_CURRENTWAY", offset_CURRENTWAY_)) + offset_CURRENTWAY_ = 0; if (freq_ <= 0.0) ROS_FATAL ("Invalid frequency param"); @@ -106,7 +114,7 @@ namespace asctec ros::NodeHandle nh_rawdata(nh_, asctec::ROS_NAMESPACE); // publish to "asctec" namespace telemetry_ = new asctec::Telemetry(nh_rawdata); - + // Diagnostics diag_updater_.add("AscTec Autopilot Status", this, &AutoPilot::diagnostics); diag_updater_.setHardwareID("none"); @@ -128,6 +136,8 @@ namespace asctec if(enable_GPS_DATA_ADVANCED_) telemetry_->enablePolling (asctec::RequestTypes::GPS_DATA_ADVANCED, interval_GPS_DATA_ADVANCED_, offset_GPS_DATA_ADVANCED_); + if(enable_CURRENTWAY_) + telemetry_->enablePolling (asctec::RequestTypes::WAYPOINT, interval_CURRENTWAY_, offset_CURRENTWAY_); // **** enable control if(enable_CONTROL_ == true) { @@ -138,7 +148,20 @@ namespace asctec { ROS_INFO("Control Disabled"); } + + // enable waypoints + if(enable_WAYPOINT_) + { + ROS_INFO("Waypoints Enabled"); + telemetry_->enableWaypoints(telemetry_); + } + else + { + ROS_INFO("Waypoints Disabled"); + } + timer_ = nh_private_.createTimer (d, &AutoPilot::spin, this); + //serialInterface_->sendWaypoint(wpcontroller_->getNextWaypoint()); } AutoPilot::~AutoPilot () @@ -158,6 +181,10 @@ namespace asctec //serialInterface_->serialport_bytes_tx_ = 0; telemetry_->publishPackets(); telemetry_->controlCount_++; + if(telemetry_->wp_received_) + { + serialInterface_->sendWaypoint(telemetry_); + } if (telemetry_->estop_) { serialInterface_->sendEstop(telemetry_); @@ -172,6 +199,7 @@ namespace asctec { serialInterface_->getPackets(telemetry_); } + last_spin_time_ = e.profile.last_duration.toSec(); diag_updater_.update(); } diff --git a/asctec_autopilot/src/serial_interface.cpp b/asctec_autopilot/src/serial_interface.cpp index 6fd923b..056a881 100644 --- a/asctec_autopilot/src/serial_interface.cpp +++ b/asctec_autopilot/src/serial_interface.cpp @@ -271,6 +271,47 @@ namespace asctec ROS_BREAK (); } } + void SerialInterface::sendWaypoint (Telemetry * telemetry) + { + int i; + char data[5]; + struct Telemetry::WAYPOINT wp = telemetry->WAYPOINT_INPUT_; + + if(!telemetry->waypointEnabled_) return; + + //ROS_DEBUG ("sendWaypoint started"); + flush(); + unsigned char cmd[] = ">*>ws"; + if(wp.chksum != (short)(wp.yaw + wp.height + wp.time + wp.X + wp.Y + wp.max_speed + wp.pos_acc + wp.properties + wp.wp_number + (short) 0xAAAA)){ + ROS_INFO("invalid WP checksum: %d != %d", wp.chksum, (short)(wp.yaw + wp.height + wp.time + wp.X + wp.Y + wp.max_speed + wp.pos_acc + wp.properties + wp.wp_number + (short) 0xAAAA)); + return; + } + output(cmd,5); + output((unsigned char*) &wp, sizeof(struct Telemetry::WAYPOINT)); + ROS_INFO("sending waypoint to pelican: size of WAYPOINT %zd", sizeof(struct Telemetry::WAYPOINT)); + wait(5); + //ROS_INFO("Data Available"); + i = read (dev_,data,5); + if (i != 5) { + ROS_ERROR("Waypoint Response : Insufficient Data"); + flush(); + return; + } + if (strncmp(data,">a",2) != 0) { + ROS_ERROR("Corrupt Response Header %c%c (%0x%0x)",data[0],data[1],data[0],data[1]); + flush(); + return; + } + if (strncmp(data+3,"a<",2) != 0) { + ROS_ERROR("Corrupt Response Footer %c%c (%0x%0x)",data[3],data[4],data[3],data[4]); + flush(); + return; + } + telemetry->wp_received_ = false; + ROS_INFO("Waypoint Response Code %0x",data[2]); + //ROS_INFO ("sendWaypoint completed" ); + } + void SerialInterface::sendControl (Telemetry * telemetry) { int i; @@ -343,9 +384,12 @@ namespace asctec ROS_DEBUG (" Requesting %04x %zd packets", (short) telemetry->requestPackets_.to_ulong (), telemetry->requestPackets_.count ()); - sprintf (cmd, ">*>p%c", (short) telemetry->requestPackets_.to_ulong ()); + //sprintf (cmd, ">*>p%c", (short) telemetry->requestPackets_.to_ulong ()); + sprintf (cmd, ">*>p"); + cmd[4] = 0xFF & telemetry->requestPackets_.to_ulong (); + cmd[5] = 0xFF & telemetry->requestPackets_.to_ulong () >> 8; output (cmd, 6); - + ROS_DEBUG (" Command: %s", cmd); for (i = 0; i < telemetry->requestPackets_.count (); i++) { packetTime = ros::Time::now(); // Presumes that the AutoPilot is grabbing the data for each packet @@ -435,6 +479,17 @@ namespace asctec } //telemetry->dumpGPS_DATA_ADVANCED(); } + else if (packet_type == Telemetry::PD_CURRENTWAY) + { + ROS_DEBUG (" Packet type is CURRENTWAY"); + memcpy (&telemetry->WAYPOINT_, spacket, packet_size); + telemetry->timestamps_[RequestTypes::WAYPOINT] = packetTime; + if (crc_valid (packet_crc, &telemetry->WAYPOINT_, packet_size)) + { + result = true; + } + //telemetry->dumpGPS_DATA_ADVANCED(); + } else { ROS_ERROR (" Packet type (%#2x) is UNKNOWN", packet_type); diff --git a/asctec_autopilot/src/telemetry.cpp b/asctec_autopilot/src/telemetry.cpp index 1186939..ab40e0c 100644 --- a/asctec_autopilot/src/telemetry.cpp +++ b/asctec_autopilot/src/telemetry.cpp @@ -44,6 +44,7 @@ namespace asctec requestCount_ = 0; pollingEnabled_ = false; requestPackets_ = 0; + wp_received_ = false; memset (requestInterval_, 0, REQUEST_TYPES * sizeof (uint8_t)); memset (requestOffset_, 0, REQUEST_TYPES * sizeof (uint8_t)); REQUEST_BITMASK[RequestTypes::LL_STATUS] = 0x0001; @@ -66,6 +67,9 @@ namespace asctec ControllerOutput_ = boost::make_shared(); GPSData_ = boost::make_shared (); GPSDataAdvanced_ = boost::make_shared (); + CurrentWay_ = boost::make_shared (); + WAYPOINT_.navigation_status=-1; + WAYPOINT_.distance_to_wp=-1; } Telemetry::~Telemetry () { @@ -82,6 +86,8 @@ namespace asctec (requestPublisher_[i].getNumSubscribers () > 0)) requestPackets_ |= REQUEST_BITMASK[i]; } + // hack for now... + requestPackets_ |= REQUEST_BITMASK[RequestTypes::WAYPOINT]; } void Telemetry::publishPackets () { @@ -130,6 +136,11 @@ namespace asctec //dumpGPS_DATA_ADVANCED(); requestPublisher_[i].publish (GPSDataAdvanced_); break; + case RequestTypes::WAYPOINT: + copyWAYPOINT (); + CurrentWay_->header.stamp = timestamps_[RequestTypes::WAYPOINT]; + requestPublisher_[i].publish (CurrentWay_); + break; default: ROS_DEBUG ("Unable to publish unknown type"); } @@ -160,7 +171,7 @@ namespace asctec requestPublisher_[msg] = nh_.advertise < asctec_msgs::GPSDataAdvanced > (requestToString (msg).c_str (), 10); break; case RequestTypes::WAYPOINT: - // to be filled in + requestPublisher_[msg] = nh_.advertise < asctec_msgs::CurrentWay > (requestToString (msg).c_str (), 10); break; case RequestTypes::CAM_DATA: // to be filled in @@ -187,6 +198,13 @@ namespace asctec controlOffset_ = offset; controlEnabled_ = true; } + void Telemetry::enableWaypoints (Telemetry * telemetry_) + { + waypointSubscriber_ = nh_.subscribe("/asctec/WAYPOINT", 1, &Telemetry::copyWAYPOINT_INPUT, telemetry_, ros::TransportHints().tcpNoDelay()); + ROS_INFO("Listening to %s data on topic: %s", "WAYPOINT","/asctec/WAYPOINT"); + ROS_DEBUG ("Telemetry::enableWaypoints()"); + waypointEnabled_ = true; + } void Telemetry::estopCallback(const std_msgs::Bool msg) { static bool info_printed = false; @@ -235,7 +253,7 @@ namespace asctec } case RequestTypes::WAYPOINT: { - return "WAYPOINT"; + return "CURRENT_WAY"; } case RequestTypes::CAM_DATA: { @@ -486,6 +504,28 @@ namespace asctec GPSDataAdvanced_->speed_x_best_estimate = GPS_DATA_ADVANCED_.speed_x_best_estimate; GPSDataAdvanced_->speed_y_best_estimate = GPS_DATA_ADVANCED_.speed_y_best_estimate; } + + void Telemetry::copyWAYPOINT () + { + CurrentWay_->navigation_status = WAYPOINT_.navigation_status; + CurrentWay_->distance_to_wp = WAYPOINT_.distance_to_wp; + } + + void Telemetry::copyWAYPOINT_INPUT(asctec_msgs::Waypoint msg){ + ROS_INFO("Received waypoint"); + WAYPOINT_INPUT_.wp_number = msg.wp_number; + WAYPOINT_INPUT_.properties = msg.properties; + WAYPOINT_INPUT_.max_speed = msg.max_speed; + WAYPOINT_INPUT_.time = msg.time; + WAYPOINT_INPUT_.pos_acc = msg.pos_acc; + WAYPOINT_INPUT_.chksum = msg.chksum; + WAYPOINT_INPUT_.X = msg.X; + WAYPOINT_INPUT_.Y = msg.Y; + WAYPOINT_INPUT_.yaw = msg.yaw; + WAYPOINT_INPUT_.height = msg.height; + wp_received_ = true; + } + void Telemetry::copyCTRL_INPUT(asctec_msgs::CtrlInput msg){ CTRL_INPUT_.pitch = msg.pitch; CTRL_INPUT_.roll = msg.roll; diff --git a/asctec_autopilot/src/waypointcontroller.cpp b/asctec_autopilot/src/waypointcontroller.cpp new file mode 100644 index 0000000..a16d257 --- /dev/null +++ b/asctec_autopilot/src/waypointcontroller.cpp @@ -0,0 +1,150 @@ +/* + * AscTec Waypoint Controller + * Copyright (C) 2014 + * Matt Sheckells + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + + + + +#include +#include +#include +#include "asctec_msgs/Waypoint.h" +#include "asctec_autopilot/waypointcontroller.h" + +namespace asctec +{ + + WaypointController::WaypointController(ros::NodeHandle nh, ros::NodeHandle nh_private): + nh_(nh), + nh_private_(nh_private), + cur_wp_(0) + { + ROS_INFO("Creating WaypointController"); + if (!nh_private_.getParam ("WAYPOINT_file", waypointFile_)) + waypointFile_ = ""; + + if(!openWayFile(waypointFile_)) + { + ROS_ERROR("Could not open waypoint file"); + } + waypointPublisher_ = nh_.advertise < asctec_msgs::Waypoint > ("/asctec/WAYPOINT", 10); + currentwaySubscriber_ = nh_.subscribe("CURRENT_WAY", 1, &WaypointController::handleCurrentway, this, ros::TransportHints().tcpNoDelay()); + + + timer_ = nh_private_.createTimer(1.0, &WaypointController::spin, this); + + } + + void WaypointController::handleCurrentway(asctec_msgs::CurrentWay msg) + { + ROS_INFO("CurrentWay navigation_status: 0x%02x", msg.navigation_status); + if(msg.navigation_status == 0x07 && hasNextWaypoint()) + { + ROS_INFO("Publishing next waypoint"); + waypointPublisher_.publish (getNextMessage()); + } + } + + void WaypointController::spin(const ros::TimerEvent & e) + { + if(hasNextWaypoint() && cur_wp_ == 0 && waypointPublisher_.getNumSubscribers() > 0) + { + ROS_INFO("Publishing first waypoint"); + waypointPublisher_.publish(getNextMessage()); + } + } + + asctec_msgs::Waypoint WaypointController::getNextMessage() + { + asctec_msgs::Waypoint wpm; + Telemetry::WAYPOINT wp = getNextWaypoint(); + + wpm.wp_number = wp.wp_number; + wpm.properties = wp.properties; + wpm.max_speed = wp.max_speed; + wpm.time = wp.time; + wpm.pos_acc = wp.pos_acc; + wpm.chksum = wp.chksum; + wpm.X = wp.X; + wpm.Y = wp.Y; + wpm.yaw = wp.yaw; + wpm.height = wp.height; + + return wpm; + } + + bool WaypointController::openWayFile(std::string filename) + { + std::ifstream f; + std::string line; + + f.open(filename.c_str()); + if(!f.is_open()) + { + ROS_ERROR("Could not open waypoint file"); + return false; + } + + std::getline(f, line); // skip header + + while(std::getline(f, line)) + { + std::vector fields; + size_t cur_pos=0; + size_t found_pos=0; + while((found_pos = line.find(';', cur_pos)) != std::string::npos) + { + fields.push_back(line.substr(cur_pos, found_pos-cur_pos)); + cur_pos = found_pos+1; + } + if(fields.size() < 7) + { + f.close(); + ROS_ERROR("Waypoint file is wrong format"); + return false; + } + struct Telemetry::WAYPOINT wp; + wp.wp_number = 1; + wp.properties = WPROP_ABSCOORDS | WPROP_HEIGHTENABLED | WPROP_AUTOMATICGOTO; + wp.max_speed = 100; + wp.time = atof(fields[5].c_str())*100; + wp.pos_acc = atof(fields[6].c_str())*1000; + wp.X = atof(fields[2].c_str())*10000000; + wp.Y = atof(fields[1].c_str())*10000000; + wp.yaw = atof(fields[4].c_str()); + wp.height = atof(fields[3].c_str())*1000; + wp.chksum = wp.yaw + wp.height + wp.time + wp.X + wp.Y + wp.max_speed + wp.pos_acc + wp.properties + wp.wp_number + (short) 0xAAAA; + + wps_.push_back(wp); + } + f.close(); + cur_wp_ = 0; + return true; + } + + bool WaypointController::hasNextWaypoint() + { + return cur_wp_ < wps_.size(); + } + + struct Telemetry::WAYPOINT WaypointController::getNextWaypoint() + { + cur_wp_++; + return wps_.at(cur_wp_-1); + } +} diff --git a/asctec_autopilot/src/waypointcontroller_node.cpp b/asctec_autopilot/src/waypointcontroller_node.cpp new file mode 100644 index 0000000..d408089 --- /dev/null +++ b/asctec_autopilot/src/waypointcontroller_node.cpp @@ -0,0 +1,30 @@ +/* + * AscTec Waypoint Controller Node + * Copyright (C) 2014 + * Matt Sheckells + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "asctec_autopilot/waypointcontroller.h" + +int main (int argc, char **argv) +{ + ros::init (argc, argv, "autopilot"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + asctec::WaypointController autopilot(nh, nh_private); + ros::spin (); + return 0; +} diff --git a/asctec_msgs/msg/CurrentWay.msg b/asctec_msgs/msg/CurrentWay.msg new file mode 100644 index 0000000..9d352ca --- /dev/null +++ b/asctec_msgs/msg/CurrentWay.msg @@ -0,0 +1,23 @@ + +# CurrentWay Message +# Copyright (C) 2014 +# Matt Sheckells +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +Header header +int16 navigation_status +int16 distance_to_wp + diff --git a/asctec_msgs/msg/Waypoint.msg b/asctec_msgs/msg/Waypoint.msg new file mode 100644 index 0000000..0984872 --- /dev/null +++ b/asctec_msgs/msg/Waypoint.msg @@ -0,0 +1,29 @@ +# CurrentWay Message +# Copyright (C) 2014 +# Matt Sheckells +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +Header header +int8 wp_number +int8 properties +int8 max_speed +int16 time +int16 pos_acc +int16 chksum +int32 X +int32 Y +int32 yaw +int32 height