Merge pull request #51 from ipa-fxm/test_of_feature

[WIP] Finalizing/Testing of TwistController features
4am-robotics · Aug 25, 2015 · bbaf88b · bbaf88b
2 parents 771ed89 + 2399687
commit bbaf88b
Show file tree

Hide file tree

Showing 120 changed files with 8,838 additions and 5,383 deletions.
diff --git a/cob_cartesian_controller/CMakeLists.txt b/cob_cartesian_controller/CMakeLists.txt
@@ -1,29 +1,60 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(cob_cartesian_controller)
 
-find_package(catkin REQUIRED COMPONENTS cmake_modules cob_srvs geometry_msgs roscpp roslib std_msgs std_srvs tf visualization_msgs)
+find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib cob_srvs geometry_msgs message_generation roscpp std_msgs std_srvs tf visualization_msgs)
 
-find_package(TinyXML REQUIRED)
+find_package(Boost REQUIRED)
+
+catkin_python_setup()
+
+### Message Generation ###
+add_action_files(
+  DIRECTORY action
+  FILES CartesianController.action
+)
+
+add_message_files(
+  DIRECTORY msg
+  FILES MoveLin.msg
+        MoveCirc.msg
+        Profile.msg
+)
+
+generate_messages(
+  DEPENDENCIES actionlib_msgs geometry_msgs std_msgs
+)
 
 catkin_package(
-  CATKIN_DEPENDS cob_srvs geometry_msgs roscpp roslib std_msgs std_srvs tf visualization_msgs
-  DEPENDS TinyXML
+  CATKIN_DEPENDS actionlib_msgs actionlib cob_srvs geometry_msgs message_runtime roscpp std_msgs std_srvs tf visualization_msgs
+  DEPENDS Boost
   INCLUDE_DIRS include
-  LIBRARIES cartesian_controller
+  LIBRARIES profile_generator trajectory_interpolator cartesian_controller cartesian_controller_utils
 )
 
 ### BUILD ###
-include_directories(include ${catkin_INCLUDE_DIRS})
+include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
+
+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_dependencies(profile_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(profile_generator ${catkin_LIBRARIES})
+
+add_library(trajectory_interpolator src/trajectory_interpolator/trajectory_interpolator.cpp)
+add_dependencies(trajectory_interpolator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(trajectory_interpolator profile_generator ${catkin_LIBRARIES})
 
 add_library(cartesian_controller src/cartesian_controller.cpp)
-add_dependencies(cartesian_controller ${catkin_EXPORTED_TARGETS})
-target_link_libraries(cartesian_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+add_dependencies(cartesian_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(cartesian_controller trajectory_interpolator cartesian_controller_utils ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 add_executable(cartesian_controller_node src/cartesian_controller_node.cpp)
 target_link_libraries(cartesian_controller_node cartesian_controller ${catkin_LIBRARIES})
 
 ### INSTALL ##
-install(TARGETS cartesian_controller cartesian_controller_node
+install(TARGETS profile_generator trajectory_interpolator cartesian_controller cartesian_controller_utils cartesian_controller_node
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -33,6 +64,10 @@ install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
 
-install(DIRECTORY launch movement
+install(PROGRAMS scripts/test_move_lin.py scripts/test_move_circ.py scripts/test_move_santa.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts
+)
+
+install(DIRECTORY config launch
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
diff --git a/cob_cartesian_controller/action/CartesianController.action b/cob_cartesian_controller/action/CartesianController.action
@@ -0,0 +1,16 @@
+# goal definition
+uint8 LIN=1
+uint8 CIRC=2
+uint8 move_type
+
+cob_cartesian_controller/MoveLin move_lin
+cob_cartesian_controller/MoveCirc move_circ
+
+---
+# result definition
+bool success
+string message
+---
+# Define a feedback message
+uint8 state
+string message
diff --git a/cob_cartesian_controller/config/example_cartesian_controller.yaml b/cob_cartesian_controller/config/example_cartesian_controller.yaml
@@ -0,0 +1,23 @@
+## joint_names
+joint_names: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+
+## Cartesian controller parameters
+chain_base_link: arm_podest_link
+chain_tip_link: arm_7_link
+root_frame: world
+
+# twist controller parameters
+twist_controller:
+  hardware_interface_type: 2    #Velocity 0, Position 1, JointStates 2
+
+# frame_tracker + interactive_marker
+frame_tracker:
+  tracking_frame: arm_7_target
+  movable_trans: true
+  movable_rot: true
+  pid_trans_x: {p: 4.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+  pid_trans_y: {p: 4.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+  pid_trans_z: {p: 4.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+  pid_rot_x: {p: 4.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+  pid_rot_y: {p: 4.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+  pid_rot_z: {p: 4.0, i: 0.0, d: 0.0, i_clamp: 0.0}
diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller.h
@@ -1,10 +1,9 @@
-
 /*!
  *****************************************************************
  * \file
  *
  * \note
- *   Copyright (c) 2014 \n
+ *   Copyright (c) 2015 \n
  *   Fraunhofer Institute for Manufacturing Engineering
  *   and Automation (IPA) \n\n
  *
@@ -18,86 +17,85 @@
  *   ROS package name: cob_cartesian_controller
  *
  * \author
- *   Author: Christoph Mark, email: [email protected]
+ *   Author: Christoph Mark, email: [email protected] / [email protected]
  *
- * \date Date of creation: August, 2014
+ * \date Date of creation: July, 2015
  *
  * \brief
  *   ...
  *
  ****************************************************************/
+
 #ifndef CARTESIAN_CONTROLLER_H
 #define CARTESIAN_CONTROLLER_H
 
+#include <ros/ros.h>
 #include <vector>
-#include <tinyxml.h>
+#include <string.h>
+#include <boost/shared_ptr.hpp>
 
-#include <ros/ros.h>
-#include <std_msgs/Float64.h>
-#include <geometry_msgs/Pose.h>
-#include <visualization_msgs/Marker.h>
-#include <tf/transform_broadcaster.h>
 #include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+
+#include <actionlib/server/simple_action_server.h>
+#include <cob_cartesian_controller/CartesianControllerAction.h>
+
+#include <cob_cartesian_controller/trajectory_interpolator/trajectory_interpolator.h>
+#include <cob_cartesian_controller/cartesian_controller_data_types.h>
+#include <cob_cartesian_controller/cartesian_controller_utils.h>
+
+typedef actionlib::SimpleActionServer<cob_cartesian_controller::CartesianControllerAction> SAS_CartesianControllerAction_t;
 
+#define DEFAULT_CARTESIAN_TARGET "cartesian_target"
 
 class CartesianController
 {
 public:
-	bool initialize();
-	void load();
-
-	// Main functions
-	void pose_path_broadcaster(std::vector <geometry_msgs::Pose> *poseVector);
-	void linear_interpolation(std::vector <geometry_msgs::Pose> *poseVector,geometry_msgs::Pose, geometry_msgs::Pose,double,double,std::string,bool);
-	void circular_interpolation(std::vector<geometry_msgs::Pose> *poseVector,double,double,double,double,double,double,double,double,double,double,double,std::string);
-	void move_ptp(geometry_msgs::Pose targetPose, double epsilon);
-	void hold_position(geometry_msgs::Pose);
-
-	// Helper function
-	bool epsilon_area(double,double,double,double,double,double,double);
-	geometry_msgs::Pose getEndeffectorPose();
-	void showMarker(tf::StampedTransform,int,double,double,double,std::string);
-	void showDot(double,double,double,int,double,double,double,std::string);
-	void showLevel(tf::Transform,int,double,double,double,std::string);
-	void timerCallback(const ros::TimerEvent&);
-	void calculateProfile(std::vector<double>*,double,double,double,std::string);
-	void calculateProfileForAngularMovements(std::vector<double> *pathMatrix,double,double,double,double,double,double,double,double,double,std::string,bool);
-	void generatePath(std::vector<double>*,double,double,double,double,int,std::string);
-	void generatePathWithTe(std::vector<double> *pathArray,double T_IPO, double te, double AcclMax,double Se_max, int steps_max,double start_angle,std::string profile);
-	void start_tracking();
-	void stop_tracking();
-	void PoseToRPY(geometry_msgs::Pose pose,double &roll, double &pitch, double &yaw);
+    bool initialize();
+
+    // Main functions
+    bool posePathBroadcaster(const geometry_msgs::PoseArray& cartesian_path);
+    bool movePTP(const geometry_msgs::Pose& target_pose, double epsilon);
+
+    // Helper function
+    bool startTracking();
+    bool stopTracking();
+
+    /// Action interface
+    void goalCallback();
+    void preemptCallback();
+    void actionSuccess(const bool success, const std::string& message);
+    void actionPreempt(const bool success, const std::string& message);
+    void actionAbort(const bool success, const std::string& message);
+
+    cob_cartesian_controller::CartesianActionStruct acceptGoal(boost::shared_ptr<const cob_cartesian_controller::CartesianControllerGoal> goal);
+    cob_cartesian_controller::MoveLinStruct convertMoveLin(const cob_cartesian_controller::MoveLin& move_lin_msg);
+    cob_cartesian_controller::MoveCircStruct convertMoveCirc(const cob_cartesian_controller::MoveCirc& move_circ_msg);
 
 private:
-	ros::NodeHandle nh_;
-
-	// Publisher
-	ros::Publisher vis_pub_;
-	ros::Publisher path_pub_;
-	ros::Publisher speed_pub_;
-	ros::Publisher accl_pub_;
-	ros::Publisher jerk_pub_;
-	ros::ServiceClient startTracking_;
-	ros::ServiceClient stopTracking_;
-
-	//TF Broadcaster-Var
-	tf::TransformBroadcaster br_;
-	tf::Transform transform_;
-	tf::Quaternion q_;
-	tf::TransformListener listener_;
-	tf::StampedTransform currentEndeffectorStampedTransform_;
-
-	// Var for PTP Movement and hold Position
-	bool reached_pos_,hold_;
-
-	// yaml params
-	double update_rate_;
-	std::string stringPath_, fileName_;
-	std::string referenceFrame_,targetFrame_;
-	std::string chain_tip_link_;
-	const char* charPath_;
-
-	int marker1_;
+    ros::NodeHandle nh_;
+    tf::TransformListener tf_listener_;
+    tf::TransformBroadcaster tf_broadcaster_;
+
+    ros::ServiceClient start_tracking_;
+    ros::ServiceClient stop_tracking_;
+    bool tracking_;
+
+    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_;
+    cob_cartesian_controller::CartesianControllerFeedback action_feedback_;
+    cob_cartesian_controller::CartesianControllerResult action_result_;
+
+    CartesianControllerUtils utils_;
+    boost::shared_ptr< TrajectoryInterpolator > trajectory_interpolator_;
 };
 
 #endif
diff --git a/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h b/cob_cartesian_controller/include/cob_cartesian_controller/cartesian_controller_data_types.h
@@ -0,0 +1,72 @@
+/*!
+ *****************************************************************
+ * \file
+ *
+ * \note
+ *   Copyright (c) 2015 \n
+ *   Fraunhofer Institute for Manufacturing Engineering
+ *   and Automation (IPA) \n\n
+ *
+ *****************************************************************
+ *
+ * \note
+ *   Project name: care-o-bot
+ * \note
+ *   ROS stack name: cob_control
+ * \note
+ *   ROS package name: cob_cartesian_controller
+ *
+ * \author
+ *   Author: Christoph Mark, email: [email protected] / [email protected]
+ *
+ * \date Date of creation: July, 2015
+ *
+ * \brief
+ *   ...
+ *
+ ****************************************************************/
+
+#ifndef COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_
+#define COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_
+
+#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;
+
+        ProfileStruct profile;
+    };
+
+    struct MoveCircStruct
+    {
+        geometry_msgs::Pose pose_center;
+        double start_angle, end_angle;
+        double radius;
+        bool rotate_only;
+
+        ProfileStruct profile;
+    };
+
+    struct CartesianActionStruct
+    {
+        unsigned int move_type;
+        MoveLinStruct move_lin;
+        MoveCircStruct move_circ;
+    };
+
+}//namespace
+
+
+#endif /* COB_CARTESIAN_CONTROLLER_DATA_STRUCTURES_H_ */