Waypoint Integration

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)

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_;

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);

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 <std_msgs/Bool.h>
+#include <bitset>
 
 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,16 +465,18 @@ 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_;
     asctec_msgs::RCDataPtr RCData_;
     asctec_msgs::ControllerOutputPtr ControllerOutput_;
     asctec_msgs::GPSDataPtr GPSData_;
     asctec_msgs::GPSDataAdvancedPtr GPSDataAdvanced_;
+    asctec_msgs::CurrentWayPtr CurrentWay_;
 
     ros::NodeHandle nh_;
     //asctec_msgs::CtrlInput CtrlInput_;

asctec_autopilot/include/asctec_autopilot/waypointcontroller.h

@@ -0,0 +1,57 @@
+/*
+ *  AscTec Waypoint Controller
+ *  Copyright (C) 2014
+ *  Matt Sheckells <[email protected]>
+ *
+ *  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 <http://www.gnu.org/licenses/>.
+ */
+
+
+#ifndef WAYPOINT_CONTROLLER_H_
+#define WAYPOINT_CONTROLLER_H_
+
+#include <ros/ros.h>
+#include <string>
+#include <vector>
+#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

asctec_autopilot/launch/log_sensors.launch

@@ -0,0 +1,30 @@
+<launch>
+
+  #### Asctec Autopilot ###################################
+
+  <node pkg="asctec_autopilot" type="autopilot_node" name="AutoPilotNode" output="screen">
+
+    <remap from="/asctec/ESTOP" to="/mav/estop"/>  
+
+    <param name="port" type="string" value="/dev/ttyS3"/>
+    <param name="speed" type="int" value="57600"/>
+    <param name="freq" type="double" value="5.0"/>
+
+    <param name="enable_IMU_CALCDATA" type="bool" value="true"/>
+    <param name="interval_IMU_CALCDATA" type="int" value="1"/>
+    <param name="offset_IMU_CALCDATA" type="int" value="0"/>
+
+    <param name="enable_LL_STATUS" type="bool" value="false"/>
+    <param name="interval_LL_STATUS" type="int" value="2"/>
+    <param name="offset_LL_STATUS" type="int" value="0"/>
+
+    <param name="enable_GPS_DATA" type="bool" value="true"/>
+    <param name="interval_GPS_DATA" type="int" value="2"/>
+    <param name="offset_GPS_DATA" type="int" value="0"/>
+  </node>
+
+  <node pkg="rosbag" type="record" name="rosbag_record"
+       args="record -o log_imu_data /asctec/IMU_CALCDATA /asctec/GPS_DATA" output="screen"
+       />
+
+</launch>

asctec_autopilot/launch/waypoint.launch

@@ -0,0 +1,53 @@
+<launch>
+
+  #### Asctec Autopilot ###################################
+
+  <node pkg="asctec_autopilot" type="autopilot_node" name="AutoPilotNode" output="screen">
+
+    <remap from="/asctec/ESTOP" to="/mav/estop"/>  
+
+    <param name="port" type="string" value="/dev/ttyS3"/>
+    <param name="speed" type="int" value="57600"/>
+    <param name="freq" type="double" value="5.0"/>
+
+    <param name="enable_IMU_CALCDATA" type="bool" value="true"/>
+    <param name="interval_IMU_CALCDATA" type="int" value="1"/>
+    <param name="offset_IMU_CALCDATA" type="int" value="0"/>
+
+    <param name="enable_LL_STATUS" type="bool" value="false"/>
+    <param name="interval_LL_STATUS" type="int" value="2"/>
+    <param name="offset_LL_STATUS" type="int" value="0"/>
+
+    <param name="enable_GPS_DATA" type="bool" value="true"/>
+    <param name="interval_GPS_DATA" type="int" value="2"/>
+    <param name="offset_GPS_DATA" type="int" value="0"/>
+
+    <param name="enable_WAYPOINT" type="bool" value="true"/>
+
+    <param name="enable_CURRENTWAY" type="bool" value="true"/>
+    <param name="interval_CURRENTWAY" type="int" value="2"/>
+    <param name="offset_CURRENTWAY" type="int" value="0"/>
+  </node>
+
+  <node pkg="asctec_autopilot" type="waypointcontroller_node" name="WaypointControllerNode" output="screen">
+    <param name="WAYPOINT_file" type="string" value="ways.csv"/>
+    <remap from="CURRENT_WAY" to="/asctec/CURRENT_WAY"/>
+  </node>
+
+  <node pkg="rosbag" type="record" name="rosbag_record"
+       args="record -o waypoint_imu_data /asctec/IMU_CALCDATA /asctec/GPS_DATA" output="screen"
+       />
+
+
+  <!--
+  #### Asctec Proc ########################################
+
+  <node pkg="asctec_proc" type="asctec_proc_node" name="AsctecProcNode" output="screen">
+
+    <param name="enable_state_changes" type="bool" value="true"/>
+    <param name="enable_ctrl_thrust"   type="bool" value="true"/>
+    <param name="enable_ctrl_yaw"      type="bool" value="true"/>
+
+  </node>
+  -->
+</launch>

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();
   }