Merge pull request #74 from ipa-fxm-cm/test_new_cartesian_controller

Finalized cob_cartesian_controller

cob_cartesian_controller/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(cob_cartesian_controller)
 
-find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib cob_srvs geometry_msgs message_generation roscpp std_msgs std_srvs tf visualization_msgs)
+find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib cob_srvs geometry_msgs message_generation roscpp roslint std_msgs std_srvs tf visualization_msgs)
 
 find_package(Boost REQUIRED)
 
@@ -38,7 +38,7 @@ add_library(cartesian_controller_utils src/cartesian_controller_utils.cpp)
 add_dependencies(cartesian_controller_utils ${catkin_EXPORTED_TARGETS})
 target_link_libraries(cartesian_controller_utils ${catkin_LIBRARIES})
 
-add_library(profile_generator src/trajectory_profile_generator/trajectory_profile_generator_lin.cpp src/trajectory_profile_generator/trajectory_profile_generator_circ.cpp)
+add_library(profile_generator src/trajectory_profile_generator/trajectory_profile_generator_builder.cpp src/trajectory_profile_generator/trajectory_profile_generator_ramp.cpp src/trajectory_profile_generator/trajectory_profile_generator_sinoid.cpp)
 add_dependencies(profile_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(profile_generator ${catkin_LIBRARIES})
 
@@ -53,6 +53,8 @@ target_link_libraries(cartesian_controller trajectory_interpolator cartesian_con
 add_executable(cartesian_controller_node src/cartesian_controller_node.cpp)
 target_link_libraries(cartesian_controller_node cartesian_controller ${catkin_LIBRARIES})
 
+roslint_cpp()
+
 ### INSTALL ##
 install(TARGETS profile_generator trajectory_interpolator cartesian_controller cartesian_controller_utils cartesian_controller_node
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

cob_cartesian_controller/action/CartesianController.action

@@ -5,7 +5,7 @@ uint8 move_type
 
 cob_cartesian_controller/MoveLin move_lin
 cob_cartesian_controller/MoveCirc move_circ
-
+cob_cartesian_controller/Profile profile
 ---
 # result definition
 bool success

cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h

@@ -19,21 +19,23 @@
  * \author
  *   Author: Christoph Mark, email: [email protected] / [email protected]
  *
- * \date Date of creation: July, 2015
+ * \date Date of creation: December, 2015
  *
  * \brief
- *   ...
+ *   This class is used to interpolate various Cartesian paths for a given velocity profile.
+ *   Supported types of Cartesian paths are Linear and Circular.
+ *   Supported types of velocity profiles are Ramp and Sinoid.
  *
  ****************************************************************/
 
-#ifndef CARTESIAN_CONTROLLER_H
-#define CARTESIAN_CONTROLLER_H
+#ifndef COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_H
+#define COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_H
 
-#include <ros/ros.h>
 #include <vector>
-#include <string.h>
+#include <string>
 #include <boost/shared_ptr.hpp>
 
+#include <ros/ros.h>
 #include <tf/transform_listener.h>
 #include <tf/transform_broadcaster.h>
 #include <tf/transform_datatypes.h>
@@ -56,7 +58,6 @@ class CartesianController
 
     // Main functions
     bool posePathBroadcaster(const geometry_msgs::PoseArray& cartesian_path);
-    bool movePTP(const geometry_msgs::Pose& target_pose, double epsilon);
 
     // Helper function
     bool startTracking();
@@ -85,9 +86,6 @@ class CartesianController
     double update_rate_;
     std::string root_frame_, chain_tip_link_, target_frame_;
 
-    // HelperVars for movePTP
-    bool reached_pos_;
-
     /// Action interface
     std::string action_name_;
     boost::shared_ptr<SAS_CartesianControllerAction_t> as_;
@@ -98,4 +96,4 @@ class CartesianController
     boost::shared_ptr< TrajectoryInterpolator > trajectory_interpolator_;
 };
 
-#endif
+#endif  // COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_H

cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h

@@ -22,51 +22,104 @@
  * \date Date of creation: July, 2015
  *
  * \brief
- *   ...
+ *   Definition of data structures used in the cob_cartesian_controller package.
  *
  ****************************************************************/
 
-#ifndef COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_
-#define COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_
+#ifndef COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_DATA_TYPES_H
+#define COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_DATA_TYPES_H
 
+#include <vector>
 #include <std_msgs/Float64.h>
 #include <geometry_msgs/Pose.h>
-#include <exception>
 
 namespace cob_cartesian_controller
 {
-    struct ProfileStruct
-    {
-        unsigned int profile_type;
-        double vel, accl;
-    };
 
-    struct MoveLinStruct
-    {
-        geometry_msgs::Pose start, end;
-        bool rotate_only;
+struct ProfileStruct
+{
+    double t_ipo;
+    unsigned int profile_type;
+    double vel, accl;
+    double Se_max;
+};
+
+struct ProfileTimings
+{
+    double tb, te, tv;
+    unsigned int steps_tb, steps_te, steps_tv;
+    double vel;
+};
+
+struct MoveLinStruct
+{
+    geometry_msgs::Pose start, end;
+};
+
+struct MoveCircStruct
+{
+    geometry_msgs::Pose pose_center;
+    double start_angle, end_angle;
+    double radius;
+};
 
-        ProfileStruct profile;
-    };
+struct CartesianActionStruct
+{
+    unsigned int move_type;
+    MoveLinStruct move_lin;
+    MoveCircStruct move_circ;
+    ProfileStruct profile;
+};
+
+class PathArray
+{
+    public:
+        PathArray(const double Se, const std::vector<double> array)
+        :    Se_(Se),
+             array_(array)
+        {}
+
+        ~PathArray()
+        {
+            array_.clear();
+        }
+
+        double Se_;
+        std::vector<double> array_;
+};
+
+class PathMatrix
+{
+    public:
+        PathMatrix(const PathArray pa1,
+                   const PathArray pa2)
+        {
+            pm_.push_back(pa1);
+            pm_.push_back(pa2);
+        }
 
-    struct MoveCircStruct
-    {
-        geometry_msgs::Pose pose_center;
-        double start_angle, end_angle;
-        double radius;
-        bool rotate_only;
+        ~PathMatrix()
+        {
+            pm_.clear();
+        }
 
-        ProfileStruct profile;
-    };
+        double getMaxSe()
+        {
+            double se_max = 0;
 
-    struct CartesianActionStruct
-    {
-        unsigned int move_type;
-        MoveLinStruct move_lin;
-        MoveCircStruct move_circ;
-    };
+            for (unsigned int i = 0; i < pm_.size(); i++)
+            {
+                if (se_max < fabs(pm_[i].Se_))
+                {
+                    se_max = fabs(pm_[i].Se_);
+                }
+            }
+            return se_max;
+        }
 
-}//namespace
+        std::vector<PathArray> pm_;
+};
 
+}  // namespace cob_cartesian_controller
 
-#endif /* COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_ */
+#endif  // COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_DATA_TYPES_H

cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_utils.h

@@ -19,30 +19,33 @@
  * \author
  *   Author: Christoph Mark, email: [email protected] / [email protected]
  *
- * \date Date of creation: July, 2015
- *
+ * \date Date of creation: December, 2015
+ *   
  * \brief
- *   ...
+ *   Helper functions  used in the cob_cartesian_controller package.
  *
  ****************************************************************/
 
-#ifndef COB_CARTESIAN_CONTROLLER_UTILS_H_
-#define COB_CARTESIAN_CONTROLLER_UTILS_H_
+#ifndef COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_UTILS_H
+#define COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_UTILS_H
+
+#include <string>
+#include <vector>
 
 #include <ros/ros.h>
 #include <tf/transform_listener.h>
 #include <tf/transform_datatypes.h>
 
 #include <geometry_msgs/PoseArray.h>
 #include <visualization_msgs/MarkerArray.h>
-
+#include <cob_cartesian_controller/cartesian_controller_data_types.h>
 
 class CartesianControllerUtils
 {
 public:
     CartesianControllerUtils()
     {
-        marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("cartesian_controller/preview_path",1);
+        marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("cartesian_controller/preview_path", 1);
     }
 
     void transformPose(const std::string source_frame, const std::string target_frame, const geometry_msgs::Pose pose_in, geometry_msgs::Pose& pose_out);
@@ -52,14 +55,18 @@ class CartesianControllerUtils
     bool inEpsilonArea(const tf::StampedTransform& stamped_transform, const double epsilon);
     void poseToRPY(const geometry_msgs::Pose& pose, double& roll, double& pitch, double& yaw);
 
-    void previewPath(const geometry_msgs::PoseArray& pose_array);
+    void previewPath(const geometry_msgs::PoseArray pose_array);
 
+    void adjustArrayLength(std::vector<cob_cartesian_controller::PathArray>& m);
+    void copyMatrix(std::vector<double>* path_array, std::vector<cob_cartesian_controller::PathArray>& m);
+    double roundUpToMultiplier(const double numberToRound, const double multiplier);
 
 private:
     ros::NodeHandle nh_;
     tf::TransformListener tf_listener_;
+    visualization_msgs::MarkerArray marker_array_;
 
     ros::Publisher marker_pub_;
 };
 
-#endif /* COB_CARTESIAN_CONTROLLER_UTILS_H_ */
+#endif  // COB_CARTESIAN_CONTROLLER_CARTESIAN_CONTROLLER_UTILS_H

cob_cartesian_controller/include/cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h

@@ -19,47 +19,45 @@
  * \author
  *   Author: Christoph Mark, email: [email protected] / [email protected]
  *
- * \date Date of creation: July, 2015
+ * \date Date of creation: December, 2015
  *
  * \brief
- *   ...
+ *   This class contains the implementation for the linear and circular interpolation.
  *
  ****************************************************************/
 
-#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H_
-#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H_
+#ifndef COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_TRAJECTORY_INTERPOLATOR_H
+#define COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_TRAJECTORY_INTERPOLATOR_H
 
+#include <string>
 #include <ros/ros.h>
 #include <geometry_msgs/PoseArray.h>
 #include <tf/transform_datatypes.h>
 
 #include <cob_cartesian_controller/cartesian_controller_data_types.h>
-#include <cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_lin.h>
-#include <cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_circ.h>
-
+#include <cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_builder.h>
 
 class TrajectoryInterpolator
 {
 public:
     TrajectoryInterpolator(std::string root_frame, double update_rate)
-    :   root_frame_(root_frame),
-        trajectory_profile_generator_lin_(TrajectoryProfileGeneratorLin(update_rate)),
-        trajectory_profile_generator_circ_(TrajectoryProfileGeneratorCirc(update_rate))
+    :   root_frame_(root_frame)
     {}
 
-    ~TrajectoryInterpolator(){}
+    ~TrajectoryInterpolator()
+    {
+        trajectory_profile_generator_.reset();
+    }
 
     bool linearInterpolation(geometry_msgs::PoseArray& pose_array,
-                             cob_cartesian_controller::MoveLinStruct& move_lin);
+                             const cob_cartesian_controller::CartesianActionStruct as);
 
     bool circularInterpolation(geometry_msgs::PoseArray& pose_array,
-                               cob_cartesian_controller::MoveCircStruct& move_circ);
+                               const cob_cartesian_controller::CartesianActionStruct as);
 
 private:
-    TrajectoryProfileGeneratorLin trajectory_profile_generator_lin_;
-    TrajectoryProfileGeneratorCirc trajectory_profile_generator_circ_;
-
     std::string root_frame_;
+    boost::shared_ptr<TrajectoryProfileBase> trajectory_profile_generator_;
 };
 
-#endif /* COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_H_ */
+#endif  // COB_CARTESIAN_CONTROLLER_TRAJECTORY_INTERPOLATOR_TRAJECTORY_INTERPOLATOR_H