initial commit. moved from robotics.ccny.cun.edu

.gitignore

@@ -0,0 +1,10 @@
+cmake_install.cmake
+build/
+bin/
+lib/
+docs/
+msg_gen/
+srv_gen/
+*.*~
+__init__.py
+__init__.pyc

CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro.  This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake.  Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake.  Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly.  CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild_make_distribution(1.1.0)

Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk

ab_filter/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#################################################################################
+
+# create ab_filter_height library
+rosbuild_add_library (ab_filter_height src/ab_filter_height.cpp)
+
+# create ab_filter_height_nodelet library
+rosbuild_add_library (ab_filter_height_nodelet src/ab_filter_height_nodelet.cpp)
+target_link_libraries (ab_filter_height_nodelet ab_filter_height)
+
+# create ab_filter_height_node executable
+rosbuild_add_executable(ab_filter_height_node src/ab_filter_height_node.cpp)
+target_link_libraries (ab_filter_height_node ab_filter_height)
+
+#################################################################################
+
+# create ab_filter_pose library
+rosbuild_add_library (ab_filter_pose src/ab_filter_pose.cpp)
+
+# create ab_filter_pose_nodelet library
+rosbuild_add_library (ab_filter_pose_nodelet src/ab_filter_pose_nodelet.cpp)
+target_link_libraries (ab_filter_pose_nodelet ab_filter_pose)
+
+# create ab_filter_height_node executable
+rosbuild_add_executable(ab_filter_pose_node src/ab_filter_pose_node.cpp)
+target_link_libraries (ab_filter_pose_node ab_filter_pose)
+
+

ab_filter/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

ab_filter/ab_filter_height_nodelet.xml

@@ -0,0 +1,8 @@
+<!-- Alpha-Beta Filter Height nodelet publisher -->
+<library path="lib/libab_filter_height_nodelet">
+  <class name="ab_filter/ABFilterHeightNodelet" type="ABFilterHeightNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      Alpha-Beta Filter Height nodelet publisher.
+    </description>
+  </class>
+</library>

ab_filter/ab_filter_pose_nodelet.xml

@@ -0,0 +1,8 @@
+<!-- Alpha-Beta Filter Pose nodelet publisher -->
+<library path="lib/libab_filter_pose_nodelet">
+  <class name="ab_filter/ABFilterPoseNodelet" type="ABFilterPoseNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      Alpha-Beta Filter Pose nodelet publisher.
+    </description>
+  </class>
+</library>

ab_filter/include/ab_filter/ab_filter_height.h

@@ -0,0 +1,76 @@
+/*
+ *  Alpha-Beta Filter
+ *  Copyright (C) 2011, CCNY Robotics Lab
+ *  Ivan Dryanovski <[email protected]>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  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 AB_FILTER_AB_FILTER_HEIGHT_H
+#define AB_FILTER_AB_FILTER_HEIGHT_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Pose.h>
+
+#include <mav_msgs/Height.h>
+
+namespace mav
+{
+
+class ABFilterHeight
+{
+  typedef mav_msgs::Height HeightMsg;
+
+  private:
+
+    // **** ROS-related
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_private_;
+
+    ros::Subscriber height_subscriber_;
+
+    ros::Publisher publisher_;
+
+    // **** state variables    
+
+    bool initialized_;
+    ros::Time last_update_time_;
+
+    HeightMsg height_;
+
+    // **** parameters
+
+    double alpha_;
+    double beta_;
+
+    // **** member functions
+
+    void initializeParams();
+
+    void heightCallback(const HeightMsg::ConstPtr height_msg);
+    void publishHeight();
+
+
+  public:
+
+    ABFilterHeight(ros::NodeHandle nh, ros::NodeHandle nh_private);
+    virtual ~ABFilterHeight();
+};
+
+} // end namespace mav
+
+#endif // AB_FILTER_AB_FILTER_HEIGHT_H

ab_filter/include/ab_filter/ab_filter_height_nodelet.h

@@ -0,0 +1,42 @@
+/*
+ *  Alpha-Beta Filter
+ *  Copyright (C) 2011, CCNY Robotics Lab
+ *  Ivan Dryanovski <[email protected]>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  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 AB_FILTER_AB_FILTER_HEIGHT_NODELET_H
+#define AB_FILTER_AB_FILTER_HEIGHT_NODELET_H
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include "ab_filter/ab_filter_height.h"
+
+namespace mav
+{
+  class ABFilterHeightNodelet : public nodelet::Nodelet
+  {
+    public:
+      virtual void onInit();
+
+    private:
+      mav::ABFilterHeight * ab_filter_;  // FIXME: change to smart pointer
+  };
+}
+
+#endif // AB_FILTER_AB_FILTER_HEIGHT_NODELET_H

ab_filter/include/ab_filter/ab_filter_pose.h

@@ -0,0 +1,99 @@
+/*
+ *  Alpha-Beta Filter
+ *  Copyright (C) 2011, CCNY Robotics Lab
+ *  Ivan Dryanovski <[email protected]>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  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 AB_FILTER_AB_FILTER_POSE_H
+#define AB_FILTER_AB_FILTER_POSE_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <tf/transform_datatypes.h>
+
+namespace mav
+{
+
+class ABFilterPose
+{
+  typedef geometry_msgs::PoseStamped Pose;
+  typedef geometry_msgs::TwistStamped Twist;
+
+// for campatibility b/n ROS Electric and Fuerte
+#if ROS_VERSION_MINIMUM(1, 8, 0)
+  typedef tf::Matrix3x3 MyMatrix;
+#else
+  typedef btMatrix3x3 MyMatrix;
+#endif
+
+  private:
+
+    // **** ROS-related
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_private_;
+
+    ros::Subscriber subscriber_;
+    ros::Publisher pose_publisher_;
+    ros::Publisher twist_publisher_;
+    ros::Publisher twist_unf_publisher_;
+
+    // **** state variables    
+
+    bool initialized_;
+    ros::Time last_update_time_;
+
+    tf::Vector3 pos_;
+    tf::Vector3 lin_vel_;
+    tf::Vector3 lin_vel_unf_;
+
+    tf::Quaternion q_;
+    tf::Vector3 ang_vel_;
+    tf::Vector3 ang_vel_unf_;
+
+    double roll_, pitch_, yaw_;
+    double v_roll_, v_pitch_, v_yaw_;
+
+    // **** parameters
+
+    double alpha_;
+    double beta_;
+    bool publish_unfiltered_;
+
+    // **** member functions
+
+    void initializeParams();
+
+    void poseCallback(const Pose::ConstPtr pose_msg);
+    void publishPose(const std_msgs::Header& header);
+
+    // [0, 2pi)
+    void normalizeAngle2Pi(double& angle);
+
+    // [-pi, pi)
+    void normalizeAnglePi(double& angle);
+
+  public:
+
+    ABFilterPose(ros::NodeHandle nh, ros::NodeHandle nh_private);
+    virtual ~ABFilterPose();
+};
+
+} // end namespace mav
+
+#endif // AB_FILTER_AB_FILTER_POSE2D_H

ab_filter/include/ab_filter/ab_filter_pose_nodelet.h

@@ -0,0 +1,42 @@
+/*
+ *  Alpha-Beta Filter
+ *  Copyright (C) 2011, CCNY Robotics Lab
+ *  Ivan Dryanovski <[email protected]>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  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 AB_FILTER_AB_FILTER_POSE_NODELET_H
+#define AB_FILTER_AB_FILTER_POSE_NODELET_H
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include "ab_filter/ab_filter_pose.h"
+
+namespace mav
+{
+  class ABFilterPoseNodelet : public nodelet::Nodelet
+  {
+    public:
+      virtual void onInit();
+
+    private:
+      mav::ABFilterPose * ab_filter_;  // FIXME: change to smart pointer
+  };
+}
+
+#endif // AB_FILTER_AB_FILTER_POSE_NODELET_H

ab_filter/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ab_filter is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/