├── application_control ├── CMakeLists.txt ├── diagnostics │ └── diagnostics_main.yaml ├── halcon_external_procedures │ ├── close_active_window.hdvp │ └── find_area_pepper.hdvp ├── include │ ├── ColdBootState.h │ ├── MoveToHomeState.h │ ├── ReadyState.h │ ├── ScanPlantState.h │ ├── SimpleActionState.h │ ├── State.h │ ├── StateMachine.h │ ├── TransitionParser.h │ └── VisualServoState.h ├── msg │ ├── State.msg │ └── changeState.msg ├── package.xml ├── readme.txt ├── servo.launch ├── src │ ├── ColdBootState.cpp │ ├── Main.cpp │ ├── MoveToHomeState.cpp │ ├── ReadyState.cpp │ ├── ScanPlantState.cpp │ └── VisualServoState.cpp ├── srv │ ├── PauseRequest.srv │ └── request_camera_parameters.srv └── transitions │ ├── transitions.dot │ └── transitions.yaml ├── fruit_detection ├── CMakeLists.txt ├── halcon_external_procedures │ └── find_pepper_features.hdvp ├── include │ ├── fruit_detection_node.h │ └── window_handling.h ├── package.xml ├── src │ └── fruit_detection_node.cpp └── srv │ └── request_features.srv ├── image_acquisition ├── CMakeLists.txt ├── camera_parameters │ ├── calculate_camera_parameter_tis_camera.hdev │ ├── calibration_model_125.descr │ └── tis_camera_parameters.dat ├── halcon_external_procedures │ ├── close_frame_grabber.hdvp │ ├── display_grabbed_image.hdvp │ ├── grab_image_resized.hdvp │ ├── grab_rectified_image.hdvp │ ├── grab_rectified_image_640_480.hdvp │ ├── grab_rectified_image_640_480_COLORSPACE.hdvp │ ├── grab_rectified_image_640_480_GRAY.hdvp │ ├── initialize_camera_tis.hdvp │ ├── initialize_camera_tis_640_480.hdvp │ └── read_camera_parameters.hdvp ├── halcon_scripts │ ├── cake_test.hdev │ ├── image_analysis.hdev │ └── image_analysis_2.hdev ├── images │ └── cake.jpg ├── include │ ├── image_acquisition_node.h │ └── window_handling.h ├── package.xml ├── src │ └── image_acquisition_node.cpp └── srv │ ├── request_camera_parameters.srv │ └── request_image.srv ├── robot_control ├── CMakeLists.txt ├── action │ └── MoveArm.action ├── package.xml ├── src │ └── robot_control_action_server.cpp └── srv │ └── request_endpointstate.srv ├── slam_bridge ├── CMakeLists.txt ├── calibration_files │ ├── pre-rectified_images.cfg │ └── readme.txt ├── include │ └── lsd_slam_pointcloud_publisher.h ├── msg │ ├── keyframeGraphMsg.msg │ └── keyframeMsg.msg ├── package.xml ├── src │ └── lsd_slam_pointcloud_publisher.cpp └── thirdparty │ └── Sophus │ ├── CMakeLists.txt │ ├── FindEigen3.cmake │ ├── README │ ├── SophusConfig.cmake.in │ ├── cmake_modules │ └── FindEigen3.cmake │ └── sophus │ ├── rxso3.hpp │ ├── se2.hpp │ ├── se3.hpp │ ├── sim3.hpp │ ├── so2.hpp │ ├── so3.hpp │ ├── sophus.hpp │ ├── test_rxso3.cpp │ ├── test_se2.cpp │ ├── test_se3.cpp │ ├── test_sim3.cpp │ ├── test_so2.cpp │ ├── test_so3.cpp │ └── tests.hpp └── visual_servo_control ├── CMakeLists.txt ├── include └── visual_servo_control_node.h ├── package.xml ├── readme ├── src ├── cycle_node.cpp └── visual_servo_control_node.cpp └── srv └── request_servo_velocity_vector.srv /application_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(application_control) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | std_msgs 7 | message_generation 8 | roscpp 9 | tf 10 | baxter_core_msgs 11 | control_msgs 12 | actionlib_msgs 13 | genmsg 14 | actionlib 15 | image_transport 16 | cv_bridge 17 | ) 18 | 19 | ## Specify additional locations of header files 20 | ## Your package locations should be listed before other locations 21 | include_directories($ENV{HALCONROOT}/include) 22 | include_directories($ENV{HALCONROOT}/include/halconcpp) 23 | include_directories($ENV{HALCONROOT}/include/hdevengine) 24 | include_directories(${catkin_INCLUDE_DIRS}) 25 | include_directories(include) 26 | 27 | 28 | ## Generate messages in the 'msg' folder 29 | add_message_files( 30 | FILES 31 | State.msg 32 | changeState.msg 33 | ) 34 | 35 | ## Generate services in the 'srv' folder 36 | add_service_files( 37 | FILES 38 | PauseRequest.srv 39 | request_camera_parameters.srv 40 | ) 41 | 42 | ## Generate added messages and services with any dependencies listed here 43 | generate_messages( 44 | DEPENDENCIES 45 | std_msgs 46 | ) 47 | 48 | 49 | catkin_package( 50 | DEPENDS yaml-cpp 51 | CATKIN_DEPENDS roscpp std_msgs control_msgs message_runtime 52 | ) 53 | 54 | add_executable(application_control_node 55 | src/Main.cpp 56 | src/ColdBootState.cpp 57 | src/ReadyState.cpp 58 | src/MoveToHomeState.cpp 59 | src/ScanPlantState.cpp 60 | src/VisualServoState.cpp 61 | ) 62 | 63 | 64 | link_directories(/opt/halcon/lib/x64-linux) 65 | 66 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 67 | 68 | target_link_libraries(application_control_node yaml-cpp ${catkin_LIBRARIES}) 69 | target_link_libraries(application_control_node $ENV{HALCONROOT}/lib/$ENV{HALCONARCH}/libhdevenginecpp.so ${catkin_LIBRARIES}) 70 | target_link_libraries(application_control_node $ENV{HALCONROOT}/lib/$ENV{HALCONARCH}/libhalconcpp.so ${catkin_LIBRARIES}) 71 | target_link_libraries(application_control_node $ENV{HALCONROOT}/lib/$ENV{HALCONARCH}/libhalcon.so ${catkin_LIBRARIES}) 72 | target_link_libraries(application_control_node yaml-cpp ${catkin_LIBRARIES}) 73 | 74 | add_dependencies(application_control_node ${catkin_EXPORTED_TARGETS}) 75 | 76 | find_package(OpenCV REQUIRED) 77 | include_directories(${OpenCV_INCLUDE_DIRS}) 78 | target_link_libraries(application_control_node ${OpenCV_LIBRARIES}) 79 | 80 | 81 | -------------------------------------------------------------------------------- /application_control/diagnostics/diagnostics_main.yaml: -------------------------------------------------------------------------------- 1 | pub_rate: 1.0 # Optional 2 | base_path: '' # Optional, prepended to all diagnostic output 3 | analyzers: 4 | State_machine: # Does not matter what name is used here (but avoid spaces!) 5 | type: diagnostic_aggregator/GenericAnalyzer # There are 4 different analyzer classes 6 | path: Main_control_program # This name will be displayed in the robot monitor 7 | startswith: Main_control_program # From the 'name' tag in the launch file 8 | find_and_remove_prefix: Main_control_program # Same name as above 9 | 10 | Error_handler: 11 | type: diagnostic_aggregator/GenericAnalyzer 12 | path: Error Manager 13 | startswith: Error_manager 14 | find_and_remove_prefix: Error_manager 15 | 16 | 17 | #Manipulator: # lets group everything concering the manpipulator here 18 | # type: AnalyzerGroup 19 | # path: Manipulator 20 | # analyzers: 21 | #Arm_controller: 22 | # type: diagnostic_aggregator/GenericAnalyzer 23 | # path: Manipulator controller 24 | # startswith: arm_control 25 | # find_and_remove_prefix: arm_control 26 | 27 | #Arm_UDP_interface: 28 | # type: diagnostic_aggregator/GenericAnalyzer 29 | # path: Arm UDP interface 30 | # startswith: crops_manipulator_udp 31 | # find_and_remove_prefix: crops_manipulator_udp 32 | 33 | -------------------------------------------------------------------------------- /application_control/halcon_external_procedures/close_active_window.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | dev_close_window () 7 | return () 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /application_control/halcon_external_procedures/find_area_pepper.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | dev_update_off() 19 | 20 | * Decompose RGB in separate channels and transform this to Hue, Saturation and Intesity channels. 21 | *decompose3 (Image, ImageR, ImageG, ImageB) 22 | *trans_from_rgb (ImageR, ImageG, ImageB, ImageResultH, ImageResultS, ImageResultI, 'hsv') 23 | * Remove noise by smoothing the Hue channel. 24 | *gauss_image (ImageResultH, ImageGauss, 9) 25 | * Segment region in color range to get all red pixels in a range. 26 | *threshold (ImageGauss, Regions, HueLowerThreshold, HueUpperThreshold) 27 | 28 | * New segmentation in cielab. 29 | decompose3 (Image, ImageR, ImageG, ImageB) 30 | trans_from_rgb (ImageR, ImageG, ImageB, ImageResult1, ImageResult2, ImageResult3, 'cielab') 31 | threshold (ImageResult2, Regions, HueLowerThreshold, HueUpperThreshold) 32 | 33 | * Remove further small noise regions. 34 | erosion_circle (Regions, RegionErosion, 2) 35 | dilation_circle (RegionErosion, RegionDilation, 4) 36 | 37 | * Find connected components in the segmented regions. 38 | connection (RegionDilation, ConnectedRegions) 39 | 40 | count_obj (ConnectedRegions, NumberOfRegions) 41 | if(NumberOfRegions > 0) 42 | area_center (ConnectedRegions, Area, Row, Column) 43 | index_largest := -1 44 | area_largest := -1 45 | for i := 1 to NumberOfRegions by 1 46 | 47 | if (Area[i-1] > area_largest) 48 | index_largest := i 49 | area_largest := Area[i-1] 50 | endif 51 | 52 | endfor 53 | select_obj(ConnectedRegions, ObjectSelected, index_largest) 54 | area_center (ObjectSelected, Area_Object, raw_cog_y, raw_cog_x) 55 | 56 | 57 | dev_get_window (CurrentWindowID) 58 | if (CurrentWindowID != -1) 59 | dev_set_window(CurrentWindowID) 60 | dev_resize_window_fit_image (Image, 0, 0, -1, -1) 61 | dev_display (Image) 62 | dev_set_color ('red') 63 | dev_display(ObjectSelected) 64 | dev_update_off() 65 | else 66 | get_image_size (Image, Width, Height) 67 | dev_open_window (0, 0, Width, Height, 'black', WindowHandle) 68 | dev_resize_window_fit_image (Image, 0, 0, -1, -1) 69 | dev_set_color ('red') 70 | dev_display(ObjectSelected) 71 | dev_display (Image) 72 | dev_update_off() 73 | endif 74 | 75 | else 76 | Area_Object := 0 77 | endif 78 | 79 | return () 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /application_control/include/ColdBootState.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ColdBoot.h 3 | */ 4 | 5 | #ifndef COLDBOOT_H_ 6 | #define COLDBOOT_H_ 7 | 8 | #include "State.h" 9 | #include "ros/ros.h" 10 | #include "diagnostic_msgs/DiagnosticArray.h" 11 | #include "std_msgs/Bool.h" 12 | #include "baxter_core_msgs/AssemblyState.h" 13 | 14 | class ColdBootState : public State 15 | { 16 | private: 17 | 18 | 19 | public: 20 | 21 | ros::NodeHandle node_handle; 22 | ros::Publisher robot_activation; 23 | ros::Subscriber robot_state_subscriber; 24 | std::string transition_message; 25 | bool robot_state_enabled; 26 | 27 | void robotStateCallback(const baxter_core_msgs::AssemblyState::ConstPtr& state_msg); 28 | ColdBootState(); 29 | std::string execute(std::map * data); 30 | }; 31 | 32 | #endif /* COLDBOOT_H_ */ 33 | -------------------------------------------------------------------------------- /application_control/include/MoveToHomeState.h: -------------------------------------------------------------------------------- 1 | #ifndef MOVETOHOMESTATE_H_ 2 | #define MOVETOHOMESTATE_H_ 3 | 4 | #include "State.h" 5 | #include "ros/ros.h" 6 | #include "std_msgs/String.h" 7 | #include "baxter_core_msgs/EndpointState.h" 8 | #include "baxter_core_msgs/SolvePositionIK.h" 9 | #include "baxter_core_msgs/JointCommand.h" 10 | #include "geometry_msgs/Pose.h" 11 | #include "geometry_msgs/Point.h" 12 | #include "geometry_msgs/Quaternion.h" 13 | #include "geometry_msgs/PoseStamped.h" 14 | #include "sensor_msgs/JointState.h" 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | class MoveToHomeState: public State { 23 | 24 | private: 25 | // Subscribers, Publishers, Clients, Listeners. 26 | tf::TransformListener transform_listener; 27 | 28 | // Global variables 29 | ros::NodeHandle node_handle; 30 | std::string transition; 31 | XmlRpc::XmlRpcValue home_position; 32 | bool first; 33 | 34 | public: 35 | void MoveToHome_Baxter(); 36 | 37 | MoveToHomeState(); 38 | std::string execute(std::map * data); 39 | }; 40 | 41 | #endif /* MOVETOHOMESTATE_H_ */ 42 | -------------------------------------------------------------------------------- /application_control/include/ReadyState.h: -------------------------------------------------------------------------------- 1 | #ifndef READYSTATE_H_ 2 | #define READYSTATE_H_ 3 | 4 | #include "State.h" 5 | #include "ros/ros.h" 6 | #include "std_msgs/String.h" 7 | 8 | class ReadyState: public State { 9 | private: 10 | ros::NodeHandle node_handle; 11 | ros::Subscriber start_subscriber ; 12 | std::string transition; 13 | public: 14 | ReadyState(); 15 | void startCallback(const std_msgs::String::ConstPtr& msg); 16 | std::string execute(std::map * data); 17 | }; 18 | 19 | #endif /* READYSTATE_H_ */ 20 | -------------------------------------------------------------------------------- /application_control/include/ScanPlantState.h: -------------------------------------------------------------------------------- 1 | #ifndef SCANPLANTSTATE_H_ 2 | #define SCANPLANTSTATE_H_ 3 | 4 | //openCV FIRST, otherwise it does not compile. Do not ask why.. 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "State.h" 11 | #include "ros/ros.h" 12 | #include "std_msgs/String.h" 13 | #include "baxter_core_msgs/EndpointState.h" 14 | #include "baxter_core_msgs/SolvePositionIK.h" 15 | #include "baxter_core_msgs/JointCommand.h" 16 | #include "geometry_msgs/Pose.h" 17 | #include "geometry_msgs/Point.h" 18 | #include "geometry_msgs/Quaternion.h" 19 | #include "geometry_msgs/PoseStamped.h" 20 | #include "sensor_msgs/JointState.h" 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include "HalconCpp.h" 29 | #include "HDevEngineCpp.h" 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | class ScanPlantState: public State { 36 | 37 | private: 38 | // Subscribers, Publishers, Clients, Listeners. 39 | tf::TransformListener transform_listener; 40 | ros::ServiceClient image_client_rgb; 41 | ros::ServiceClient robot_endpointstate_client; 42 | ros::ServiceClient request_features_client; 43 | ros::Subscriber robot_endpoint_state_subscriber; 44 | 45 | // Global variables 46 | ros::NodeHandle node_handle; 47 | std::string transition; 48 | geometry_msgs::PoseStamped robot_endpoint_pose; 49 | bool first; 50 | XmlRpc::XmlRpcValue scan_waypoint_1; 51 | XmlRpc::XmlRpcValue scan_waypoint_2; 52 | XmlRpc::XmlRpcValue scan_waypoint_3; 53 | XmlRpc::XmlRpcValue scan_waypoint_4; 54 | 55 | HDevEngineCpp::HDevEngine hdevengine; 56 | std::string state_machine_halcon_external_procedures_path; 57 | std::string find_area_pepper_function; 58 | std::string close_window_function; 59 | int hue_lower_threshold; 60 | int hue_upper_threshold; 61 | 62 | struct detected_fruit { 63 | int area; 64 | geometry_msgs::PoseStamped pose; 65 | } ; 66 | 67 | std::vector detected_fruits; 68 | detected_fruit best_fruit; 69 | 70 | 71 | public: 72 | 73 | ScanPlantState(); 74 | std::string execute(std::map * data); 75 | geometry_msgs::PoseStamped requestEndPointState(); 76 | void EndpointStateCallback_Baxter(const baxter_core_msgs::EndpointState::ConstPtr& endpoint_msg); 77 | bool detectFruit(); 78 | void closeWindow(); 79 | std::string goToScanWaypoint(XmlRpc::XmlRpcValue waypoint); 80 | void setVisualServoStartingPose(std::map * data); 81 | 82 | }; 83 | 84 | #endif /* SCANPLANTSTATE_H_ */ 85 | -------------------------------------------------------------------------------- /application_control/include/SimpleActionState.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SimpleActionState.h 3 | * 4 | * Created on: Mar 1, 2012 5 | * Author: Peter Hohnloser 6 | */ 7 | 8 | #ifndef SIMPLEACTIONSTATE_H_ 9 | #define SIMPLEACTIONSTATE_H_ 10 | #include 11 | #include 12 | #include 13 | #include "actionlib/client/simple_goal_state.h" 14 | #include "actionlib/client/simple_client_goal_state.h" 15 | #include 16 | #include 17 | 18 | using std::string; 19 | 20 | /* The class SimpleActionState is an abstract class which means 21 | * that another class has to inherit from it. 22 | */ 23 | 24 | template 25 | class SimpleActionState : public State 26 | { 27 | protected: 28 | ACTION_DEFINITION(ASpec); 29 | typedef actionlib::SimpleActionClient client_; 30 | AGoal goal_; 31 | client_ ac_; 32 | bool first_; 33 | public: 34 | typedef boost::function 35 | SimpleDoneCallback; 36 | typedef boost::function SimpleFeedbackCallback; 37 | typedef boost::function SimpleActiveCallback; 38 | 39 | /* 40 | * Constructor needs the name of the SimpleActionServer node 41 | */ 42 | 43 | SimpleActionState(string serverNode) : 44 | ac_(serverNode, true) 45 | { 46 | feedback_cb_ = SimpleFeedbackCallback(); 47 | done_cb_ = SimpleDoneCallback(); 48 | first_ = true; 49 | ROS_INFO("starting to wait for server %s",serverNode.c_str()); 50 | ac_.waitForServer(ros::Duration(5)); 51 | if (ac_.isServerConnected()) 52 | ROS_INFO("connected to server %s",serverNode.c_str()); 53 | else 54 | ROS_WARN("#GUI NOT!!! connected to server %s",serverNode.c_str()); 55 | 56 | } 57 | 58 | virtual ~SimpleActionState() 59 | { 60 | } 61 | /** 62 | * sends the call to the SimpleActionServer 63 | */ 64 | void sendGoal() 65 | { 66 | ac_.sendGoal(goal_, done_cb_, SimpleActiveCallback(), feedback_cb_); 67 | ac_.waitForResult(ros::Duration(0.1)); 68 | } 69 | /** 70 | * 71 | */ 72 | virtual bool setPause(bool p) 73 | { 74 | /* 75 | if (p) 76 | { 77 | if (!first_) 78 | { 79 | //ac_.cancelGoal(); 80 | } 81 | } 82 | else 83 | { 84 | ac_.sendGoal(goal_, done_cb_, SimpleActiveCallback(), feedback_cb_); 85 | ac_.waitForResult(ros::Duration(0.1)); 86 | } 87 | */ 88 | mx.lock(); 89 | pause = p; 90 | mx.unlock(); 91 | 92 | return true; 93 | } 94 | /** 95 | * sets the feedback callback method for the SimpleActionCleint 96 | */ 97 | void setFeedbackCallback(SimpleFeedbackCallback feedback_cb) 98 | { 99 | feedback_cb_ = feedback_cb; 100 | } 101 | /* 102 | * set the done callback method for the SimpleActinClient 103 | */ 104 | void setDoneCallback(SimpleDoneCallback done_cb) 105 | { 106 | done_cb_ = done_cb; 107 | } 108 | 109 | void stop(){ 110 | if(!first_) 111 | { 112 | ac_.cancelGoal(); 113 | } 114 | first_ = true; 115 | } 116 | 117 | private: 118 | SimpleDoneCallback done_cb_; 119 | SimpleFeedbackCallback feedback_cb_; 120 | }; 121 | 122 | #endif /* SIMPLEACTIONSTATE_H_ */ 123 | -------------------------------------------------------------------------------- /application_control/include/State.h: -------------------------------------------------------------------------------- 1 | /* 2 | * State.h 3 | * 4 | * Created on: Mar 1, 2012 5 | * Author: Peter Hohnloser 6 | */ 7 | #ifndef STATE_H 8 | #define STATE_H 9 | 10 | #include 11 | #include "string.h" 12 | #include 13 | #include 14 | 15 | 16 | using std::string; 17 | using std::map; 18 | using boost::any; 19 | using boost::mutex; 20 | /* 21 | * The class state is an abstract class which means 22 | * that another class has to inherit from it. 23 | */ 24 | class State 25 | { 26 | private: 27 | string state_name_; 28 | // ros::NodeHandle nh_; 29 | 30 | protected: 31 | bool pause; 32 | mutex mx; 33 | ros::Publisher stat_pub_; 34 | ros::NodeHandle nh_; 35 | 36 | public: 37 | 38 | /* 39 | *Constructs a state which is used by the state machine 40 | */ 41 | State() 42 | { 43 | pause = false; 44 | mutex::scoped_lock mylock(mx, boost::defer_lock); 45 | } 46 | ; 47 | 48 | virtual ~State() 49 | { 50 | } 51 | ; 52 | 53 | /** 54 | * -------------------- These methods are used in the state machine------- 55 | */ 56 | 57 | /* 58 | * Sets the name for the state. 59 | */ 60 | void setStateName(string state_name) 61 | { 62 | state_name_ = state_name; 63 | } 64 | ; 65 | /* 66 | * returns the name of the state 67 | */ 68 | string getStateName() 69 | { 70 | return state_name_; 71 | } 72 | ; 73 | 74 | /* 75 | * This method is needed in the state machine for executing a state. 76 | * This method must be implemented in the class which inherits from State. 77 | */ 78 | virtual string execute(map * statedata)=0; 79 | 80 | /* 81 | * This method sets the state to pause. 82 | * It is possible to override this method for customize what happens when the state gets a pause request. 83 | */ 84 | virtual bool setPause(bool p) 85 | { 86 | mx.lock(); 87 | pause = p; 88 | mx.unlock(); 89 | return true; 90 | } 91 | /** 92 | * This method returns true if the state is paused, otherwise false 93 | */ 94 | bool getPause() 95 | { 96 | mx.lock(); 97 | bool temp = pause; 98 | mx.unlock(); 99 | return temp; 100 | } 101 | 102 | virtual void stop(){}; 103 | 104 | }; 105 | 106 | #endif 107 | -------------------------------------------------------------------------------- /application_control/include/StateMachine.h: -------------------------------------------------------------------------------- 1 | /* 2 | * StateMachine.h 3 | * 4 | * Created on: Mar 1, 2012 5 | * Author: Peter Hohnloser 6 | */ 7 | #ifndef STATEMACHINE_H 8 | #define STATEMACHINE_H 9 | #include 10 | #include "State.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using std::map; 25 | using std::string; 26 | using boost::thread; 27 | using boost::mutex; 28 | 29 | class StateMachine 30 | { 31 | private: 32 | volatile bool run_; 33 | ros::NodeHandle n_; 34 | ros::Publisher curr_state_pub_; 35 | map > transitions_; 36 | map state_description_; 37 | map states_; 38 | map state_data_; 39 | State * current_state_; 40 | string *out_; 41 | string next_state_; 42 | string start_state_; 43 | int outSize_; 44 | thread rosspin_; 45 | double frequency_; 46 | double pause_start_time_; 47 | mutex next; 48 | // Diagnostics: 49 | double min_freq_, max_freq_; 50 | public: 51 | 52 | /** 53 | * Constructor for a State Machine 54 | * 55 | */ 56 | StateMachine(string outcomes[], int outcomeSize); 57 | 58 | virtual ~StateMachine() 59 | { 60 | } 61 | ; 62 | /** 63 | * Adds a State to the State Machine 64 | */ 65 | void add(string label, State *state); 66 | 67 | // void add(string label, State *state,string transition[], int n); 68 | /* 69 | * adds the yaml file to the state machine and creates the transition between the states 70 | */ 71 | void addTransitionFile(string file_name); 72 | /** 73 | * executes the State Machine 74 | */ 75 | string execute(); 76 | 77 | private: 78 | /** 79 | * Checks the transitions between states 80 | */ 81 | void checkTransitions(); 82 | /* 83 | * initialize ros and creates a pause request service thread 84 | * 85 | */ 86 | void init(); 87 | /* 88 | * method which a pause request service thread carries out 89 | */ 90 | void rosSpin(); 91 | /* 92 | * Callback method for the pause request service which sets a states to pause 93 | */ 94 | bool pause(application_control::PauseRequest::Request &rep, application_control::PauseRequest::Response &res); 95 | 96 | /** 97 | * Callback methed for changing a state 98 | */ 99 | 100 | void changeState(const application_control::changeState::ConstPtr& msg); 101 | 102 | }; 103 | 104 | 105 | StateMachine::StateMachine(string outcomes[], int outcomeSize) 106 | { 107 | out_ = outcomes; 108 | outSize_ = outcomeSize; 109 | init(); 110 | } 111 | 112 | void StateMachine::add(string label, State *state) 113 | { 114 | state->setStateName(label); 115 | states_[label] = state; 116 | } 117 | 118 | string StateMachine::execute() 119 | { 120 | string last_state_, trans_; 121 | application_control::State state_msg; 122 | int i = 0; 123 | bool hasFinishd_ = false; 124 | checkTransitions(); 125 | ros::Rate loop_rate(100); 126 | current_state_ = states_[start_state_]; 127 | double start_time = ros::Time::now().toSec(); 128 | while (ros::ok()) 129 | { 130 | 131 | if (!current_state_->getPause()) 132 | { 133 | state_msg.description = state_description_[current_state_->getStateName()]; 134 | state_msg.name = current_state_->getStateName(); 135 | state_msg.header.stamp = ros::Time::now(); 136 | ROS_INFO("Current State: %s", state_msg.name.c_str()); 137 | curr_state_pub_.publish(state_msg); 138 | last_state_ = current_state_->getStateName(); 139 | trans_ = current_state_->execute(&state_data_); 140 | 141 | if (transitions_[last_state_].find(trans_) == transitions_[last_state_].end()) 142 | { 143 | ROS_ERROR("#GUI State %s has no outcome %s ", last_state_.c_str(), trans_.c_str()); 144 | run_ = false; 145 | rosspin_.join(); 146 | exit(-1); 147 | } 148 | 149 | next_state_ = transitions_[last_state_][trans_]; 150 | for (i = 0; i < outSize_; i++) 151 | { 152 | if (next_state_.compare(out_[i]) == 0) 153 | { 154 | hasFinishd_ = true; 155 | break; 156 | } 157 | } 158 | if (hasFinishd_) 159 | { 160 | break; 161 | } 162 | next.unlock(); 163 | loop_rate.sleep(); 164 | next.lock(); 165 | if (current_state_->getStateName().compare(next_state_) != 0) 166 | { 167 | //ROS_INFO("#SUM %s Duration %f sec", current_state_->getStateName().c_str(),ros::Time::now().toSec() - start_time); 168 | start_time = ros::Time::now().toSec(); 169 | } 170 | current_state_ = states_[next_state_]; 171 | } 172 | else 173 | { 174 | loop_rate.sleep(); 175 | } 176 | } 177 | state_msg.description = "State machine has finished"; 178 | state_msg.name = "Exit"; 179 | curr_state_pub_.publish(state_msg); 180 | run_ = false; 181 | rosspin_.join(); 182 | ROS_INFO("State Machine exit"); 183 | return out_[i]; 184 | } 185 | 186 | void StateMachine::addTransitionFile(string file_name) 187 | { 188 | TransitionParser tran; 189 | transitions_ = tran.parser(file_name, &state_description_, &start_state_); 190 | } 191 | 192 | /** 193 | *---------------------------------------------------------------Private methods-------------------------------------------------------------------- 194 | */ 195 | 196 | void StateMachine::checkTransitions() 197 | { 198 | map >::iterator it; 199 | map::iterator it2; 200 | int i; 201 | bool hasFailure_ = true; 202 | if (states_.find(start_state_) == states_.end()) 203 | { 204 | ROS_ERROR("#GUI State machine has no start state %s", start_state_.c_str()); 205 | ros::shutdown(); 206 | } 207 | 208 | for (it = transitions_.begin(); it != transitions_.end(); it++) 209 | { 210 | map &temp = it->second; 211 | for (it2 = temp.begin(); it2 != temp.end(); it2++) 212 | { 213 | 214 | if (states_.find(it2->second) == states_.end()) 215 | { 216 | ROS_INFO("State Machine: state label [%s] outcome [%s] to [%s]", it->first.c_str(), it2->first.c_str(), 217 | it2->second.c_str()); 218 | for (i = 0; i < outSize_; i++) 219 | { 220 | if (out_[i].compare(it2->second) == 0) 221 | { 222 | hasFailure_ = false; 223 | break; 224 | } 225 | } 226 | if (hasFailure_) 227 | { 228 | ROS_ERROR("#GUI State ( %s ) outcome ( %s ) has no transition to next State ( %s )", it->first.c_str(), 229 | it2->first.c_str(), it2->second.c_str()); 230 | ros::shutdown(); 231 | } 232 | hasFailure_ = true; 233 | } 234 | } 235 | } 236 | } 237 | 238 | void StateMachine::init() 239 | { 240 | char *argv[1]; 241 | int argc = 1; 242 | argv[0] = get_current_dir_name(); 243 | run_ = true; 244 | ros::init(argc, argv, "StateMachine"); 245 | curr_state_pub_ = n_.advertise("current_state", 1); 246 | rosspin_ = thread(&StateMachine::rosSpin, this); 247 | ROS_INFO("State Machine initialized"); 248 | } 249 | 250 | void StateMachine::rosSpin() 251 | { 252 | ROS_INFO("State Machine I'm alive message started"); 253 | ros::ServiceServer service_ = n_.advertiseService("pause_request", &StateMachine::pause, this); 254 | ros::Subscriber sub_ = n_.subscribe("change_state", 1, &StateMachine::changeState, this); 255 | ros::Publisher alive_pub_ = n_.advertise("update", 1); 256 | ros::Rate loop_rate(100); 257 | std_msgs::String msg; 258 | msg.data = "I'm alive"; 259 | while (run_) 260 | { 261 | ros::spinOnce(); 262 | alive_pub_.publish(msg); 263 | ros::Time timestamp = ros::Time::now(); 264 | loop_rate.sleep(); 265 | } 266 | } 267 | 268 | bool StateMachine::pause(application_control::PauseRequest::Request &rep, application_control::PauseRequest::Response &res) 269 | { 270 | ROS_INFO("State Machine pause requested"); 271 | res.succeded = true; 272 | 273 | if (current_state_->setPause(rep.pause)) 274 | { 275 | res.succeded = false; 276 | } 277 | 278 | if (rep.pause) 279 | { 280 | // start pause timer 281 | pause_start_time_ = ros::Time::now().toSec(); 282 | } 283 | else 284 | { 285 | // end pause timer and log duration to file 286 | ROS_INFO("#SUM Pause duration %f sec, (state: %s)", ros::Time::now().toSec() - pause_start_time_, 287 | current_state_->getStateName().c_str()); 288 | } 289 | return true; 290 | } 291 | 292 | void StateMachine::changeState(const application_control::changeState::ConstPtr& msg) 293 | { 294 | ROS_INFO("State Machine change state to %s", msg->state_label.c_str()); 295 | map::iterator it; 296 | current_state_->stop(); 297 | for (it = states_.begin(); it != states_.end(); it++) 298 | { 299 | it->second->stop(); 300 | } 301 | current_state_->setPause(false); 302 | next.lock(); 303 | next_state_ = msg->state_label; 304 | next.unlock(); 305 | 306 | } 307 | 308 | #endif // STATEMACHINE_H 309 | -------------------------------------------------------------------------------- /application_control/include/TransitionParser.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TransitionPaser.h 3 | * 4 | * Created on: Mar 22, 2012 5 | * Author: Peter Hohnloser 6 | */ 7 | 8 | #ifndef TRANSITIONPARSER_H_ 9 | #define TRANSITIONPARSER_H_ 10 | 11 | #include "yaml-cpp/yaml.h" 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using std::map; 25 | using std::string; 26 | using std::vector; 27 | using namespace YAML; 28 | 29 | /** 30 | * Class TransitionParser parses a yaml file into the transitions between states 31 | */ 32 | class TransitionParser 33 | { 34 | public: 35 | /* 36 | * Constructor 37 | */ 38 | TransitionParser() 39 | { 40 | } 41 | ; 42 | /* 43 | * parses a yaml file into a map with keys of strings and mapped values as a(nother) map of string keys and a string as the mapped value 44 | */ 45 | map > parser(string file_name, map * state_description, 46 | string *strat_state); 47 | }; 48 | 49 | 50 | map > TransitionParser::parser(string file_name, map * state_description, 51 | string *start_state) 52 | { 53 | string path(file_name.substr(0, file_name.size() - 4)); 54 | path += "dot"; 55 | ROS_INFO("transition file path: %s", file_name.c_str()); 56 | ROS_INFO("dot file path: %s ", path.c_str()); 57 | std::ofstream dotfile_(path.c_str()); 58 | dotfile_ << "digraph{\n"; 59 | 60 | map > transitions_; 61 | try 62 | { 63 | ROS_DEBUG("File_name: %s", file_name.c_str()); 64 | Node doc = LoadFile(file_name); 65 | ROS_DEBUG("YAML FILE LODED "); 66 | (*start_state) = doc[0]["start_state"].as().c_str(); 67 | for (int i = 1; i < doc.size(); i++) 68 | { 69 | (*state_description)[doc[i]["state"]["state_label"].as()] = 70 | doc[i]["state"]["descriptive_name"].as(); 71 | ROS_DEBUG("%s", doc[i]["state"]["descriptive_name"].as().c_str()); 72 | ROS_DEBUG("%s", doc[i]["state"]["state_label"].as().c_str()); 73 | Node trans = doc[i]["state"]["transitions"]; 74 | for (const_iterator it = trans.begin(); it != trans.end(); ++it) 75 | { 76 | transitions_[doc[i]["state"]["state_label"].as()][it->first.as()] = it->second.as(); 77 | ROS_DEBUG("%s : %s", it->first.as().c_str(), it->second.as().c_str()); 78 | dotfile_ << doc[i]["state"]["state_label"].as() << "->" << it->second.as() << " [ label=" << '"' 79 | << it->first << '"' << " ];" << "\n"; 80 | } 81 | } 82 | } 83 | catch (ParserException& e) 84 | { 85 | ROS_ERROR("Error parsing the yaml-file. Do not use tabs, only spaces"); 86 | ros::shutdown(); 87 | } 88 | dotfile_ << "}\n"; 89 | dotfile_.close(); 90 | ROS_INFO("%s file created", path.c_str()); 91 | string execute("dot -Tps "); 92 | execute += path; 93 | execute += " -o "; 94 | execute += path.substr(0, path.size() - 4); 95 | execute += ".pdf"; 96 | if (0 != system(execute.c_str())) 97 | { 98 | ROS_ERROR("Couldn't create a pdf from %s.", path.c_str()); 99 | } 100 | 101 | return transitions_; 102 | } 103 | 104 | #endif /* TRANSITIONPASER_H_ */ 105 | -------------------------------------------------------------------------------- /application_control/include/VisualServoState.h: -------------------------------------------------------------------------------- 1 | #ifndef VISUALSERVOSTATE_H_ 2 | #define VISUALSERVOSTATE_H_ 3 | 4 | #include "State.h" 5 | #include "ros/ros.h" 6 | #include "std_msgs/String.h" 7 | #include "std_msgs/Time.h" 8 | #include "std_msgs/Bool.h" 9 | 10 | #include "baxter_core_msgs/EndpointState.h" 11 | #include "baxter_core_msgs/SolvePositionIK.h" 12 | #include "baxter_core_msgs/JointCommand.h" 13 | 14 | #include "geometry_msgs/Pose.h" 15 | #include "geometry_msgs/Point.h" 16 | #include "geometry_msgs/Quaternion.h" 17 | #include "geometry_msgs/PoseStamped.h" 18 | #include "sensor_msgs/JointState.h" 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | 33 | class VisualServoState: public State { 34 | 35 | private: 36 | 37 | // Subscribers, Publishers, Clients, Listeners. 38 | tf::TransformListener transform_listener; 39 | ros::ServiceClient robot_endpointstate_client; 40 | ros::ServiceClient servo_velocity_vector_client; 41 | ros::ServiceClient request_features_client; 42 | 43 | ros::ServiceClient image_client_rgb; 44 | ros::ServiceClient image_client_gray; 45 | ros::Subscriber robot_endpoint_state_subscriber; 46 | ros::Publisher cancel_robot_movement_publisher; 47 | ros::Publisher servo_setgoal_publisher; 48 | ros::Publisher servo_startstop_publisher; 49 | ros::Publisher servo_execute_publisher; 50 | 51 | // Global variables. 52 | ros::NodeHandle node_handle; 53 | geometry_msgs::PoseStamped robot_endpoint_pose; 54 | std::string transition; 55 | double traveled_servo_distance; 56 | bool external_servo_loop_running; 57 | bool internal_servo_loop_running; 58 | double forward_distance_step; 59 | std_msgs::Time most_recent_image_timestamp; 60 | 61 | 62 | public: 63 | 64 | VisualServoState(); 65 | std::string execute(std::map * data); 66 | void EndpointStateCallback_Baxter(const baxter_core_msgs::EndpointState::ConstPtr& endpoint_msg); 67 | void moveArm(std::vector velocity_vector, float error_x, float error_y, std_msgs::Time image_time); 68 | geometry_msgs::PoseStamped requestEndPointState(); 69 | }; 70 | 71 | #endif /* VISUALSERVOSTATE_H_ */ 72 | -------------------------------------------------------------------------------- /application_control/msg/State.msg: -------------------------------------------------------------------------------- 1 | # Message containing the current state from the state machine 2 | Header header 3 | string name 4 | string description 5 | -------------------------------------------------------------------------------- /application_control/msg/changeState.msg: -------------------------------------------------------------------------------- 1 | # Message containing the state label from the next state in the state machine 2 | string state_label -------------------------------------------------------------------------------- /application_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | application_control 3 | 1.0.0 4 | Main control program, using a state machine. 5 | 6 | BSD 7 | 8 | Ruud Barth 9 | 10 | 11 | catkin 12 | 13 | 14 | message_generation 15 | message_runtime 16 | 17 | 18 | std_msgs 19 | roscpp 20 | tf 21 | yaml-cpp 22 | baxter_core_msgs 23 | control_msgs 24 | 25 | actionlib 26 | actionlib_msgs 27 | actionlib 28 | actionlib_msgs 29 | 30 | opencv2 31 | cv_bridge 32 | image_transport 33 | opencv2 34 | cv_bridge 35 | image_transport 36 | 37 | std_msgs 38 | roscpp 39 | tf 40 | yaml-cpp 41 | baxter_core_msgs 42 | control_msgs 43 | 44 | 45 | -------------------------------------------------------------------------------- /application_control/readme.txt: -------------------------------------------------------------------------------- 1 | 2 | To start the state machine from the command line when in the ReadyState: 3 | 4 | $ rostopic pub /state_machine_start std_msgs/String "start state machine" 5 | 6 | -------------------------------------------------------------------------------- /application_control/servo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | [ 0.70, -0.90, 0.45, 0.0 , 0.71 , -0.0 , 0.71 ] 54 | [ 0.86, -0.50, 0.45, 0.30 , 0.71 , -0.30 , 0.71 ] 55 | [ 0.70, -0.90, 0.45, 0.0 , 0.71 , -0.0 , 0.71 ] 56 | [ 0.70, -0.90, 0.45, 0.0 , 0.71 , -0.0 , 0.71 ] 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /application_control/src/ColdBootState.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014,Umeå University 5 | * Copyright (c) 2014,DLO, Wageningen University & Research Center. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Open Source Robotics Foundation nor 19 | * the names of its contributors may be used to endorse or promote 20 | * products derived from this software without specific prior 21 | * written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #include "ColdBootState.h" 38 | 39 | 40 | void ColdBootState::robotStateCallback(const baxter_core_msgs::AssemblyState::ConstPtr& state_msg) 41 | { 42 | // If robot is already enabled, then do nothing. 43 | // The robot keeps triggering this callback. 44 | if(!robot_state_enabled) 45 | { 46 | // If the robot is enabled, set global variable to true. 47 | if(state_msg->enabled) 48 | { 49 | ROS_INFO("Robot enabled"); 50 | robot_state_enabled = true; 51 | } 52 | else 53 | { 54 | ROS_INFO("Robot not yet enabled, trying to enable."); 55 | robot_activation = node_handle.advertise("/robot/set_super_enable", 1000); 56 | std_msgs::Bool activation; 57 | activation.data = true; 58 | robot_activation.publish(activation); 59 | } 60 | } 61 | } 62 | 63 | 64 | 65 | ColdBootState::ColdBootState() 66 | { 67 | // System is not ready by default. 68 | robot_state_enabled = false; 69 | 70 | // Callback check if Baxter robot is activated. 71 | robot_state_subscriber = node_handle.subscribe("/robot/state", 1000, &ColdBootState::robotStateCallback, this); 72 | } 73 | 74 | 75 | 76 | std::string ColdBootState::execute(std::map * data) 77 | { 78 | // Check if all ready bools are true. 79 | // If so, then the Coldboot is done. 80 | if(robot_state_enabled) 81 | { 82 | transition_message = "ready"; 83 | } 84 | else 85 | { 86 | transition_message = "not ready"; 87 | } 88 | ros::Rate rate(1.0); 89 | rate.sleep(); 90 | 91 | transition_message = "ready"; 92 | return transition_message; 93 | } 94 | -------------------------------------------------------------------------------- /application_control/src/Main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014,Umeå University 5 | * Copyright (c) 2014,DLO, Wageningen University & Research Center. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Open Source Robotics Foundation nor 19 | * the names of its contributors may be used to endorse or promote 20 | * products derived from this software without specific prior 21 | * written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #include 39 | #include 40 | 41 | // Include states 42 | #include "ColdBootState.h" 43 | #include "ReadyState.h" 44 | #include "MoveToHomeState.h" 45 | #include "ScanPlantState.h" 46 | #include "VisualServoState.h" 47 | 48 | using std::string; 49 | 50 | int main(int argc, char** argv) 51 | { 52 | ros::init(argc, argv, ""); 53 | 54 | ROS_INFO("Main servo program started with transition file %s", argv[1]); 55 | 56 | // Create State objects 57 | ColdBootState cold_boot_state; 58 | ReadyState ready_state; 59 | MoveToHomeState move_to_home_state; 60 | ScanPlantState scan_plant_state; 61 | VisualServoState visual_servo_state; 62 | 63 | // Define an outcome of the state machine (main program will exit) 64 | string sm_outcome[] = {"Exit"}; 65 | StateMachine sm(sm_outcome, 1); 66 | 67 | // Transitions file (defined in the launchfile) 68 | sm.addTransitionFile(argv[1]); 69 | 70 | std::string tfile = (std::string)argv[1]; 71 | 72 | // Add states to the state machine 73 | sm.add("ColdBootState", &cold_boot_state); 74 | sm.add("ReadyState", &ready_state); 75 | sm.add("MoveToHomeState", &move_to_home_state); 76 | sm.add("ScanPlantState", &scan_plant_state); 77 | sm.add("VisualServoState", &visual_servo_state); 78 | 79 | // Start state machine. 80 | sm.execute(); 81 | 82 | ROS_INFO("State Machine Finished"); 83 | 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /application_control/src/MoveToHomeState.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014,Umeå University 5 | * Copyright (c) 2014,DLO, Wageningen University & Research Center. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Open Source Robotics Foundation nor 19 | * the names of its contributors may be used to endorse or promote 20 | * products derived from this software without specific prior 21 | * written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | #include "MoveToHomeState.h" 37 | 38 | 39 | MoveToHomeState::MoveToHomeState() 40 | { 41 | // Initially the goal is not reached. 42 | transition = "goal not reached"; 43 | 44 | // Get home position from launch file. 45 | node_handle.getParam("home_position", home_position); 46 | 47 | first = true; 48 | } 49 | 50 | // TODO: select arm from launch file parameter. 51 | std::string MoveToHomeState::execute(std::map * data) 52 | { 53 | // Create the action client for arm movement. True causes the client to spin its own thread. 54 | actionlib::SimpleActionClient action_client_right("robot_control_right_arm", true); 55 | 56 | ROS_INFO("Waiting for action server for moving rigth Baxter arm to start."); 57 | action_client_right.waitForServer(); //will wait for infinite time 58 | ROS_INFO("Action server for moving rigth Baxter arm is started."); 59 | 60 | ROS_INFO("Sending goal."); 61 | robot_control::MoveArmGoal goal_message; 62 | goal_message.goal_pose.header.stamp = ros::Time::now(); 63 | goal_message.goal_pose.header.frame_id = "base"; 64 | goal_message.goal_pose.pose.position.x = home_position[0]; 65 | goal_message.goal_pose.pose.position.y = home_position[1]; 66 | goal_message.goal_pose.pose.position.z = home_position[2]; 67 | goal_message.goal_pose.pose.orientation.x = home_position[3]; 68 | goal_message.goal_pose.pose.orientation.y = home_position[4]; 69 | goal_message.goal_pose.pose.orientation.z = home_position[5]; 70 | goal_message.goal_pose.pose.orientation.w = home_position[6]; 71 | 72 | action_client_right.sendGoal(goal_message); 73 | 74 | actionlib::SimpleClientGoalState state = actionlib::SimpleClientGoalState::ACTIVE; 75 | while(state == actionlib::SimpleClientGoalState::PENDING || state == actionlib::SimpleClientGoalState::ACTIVE) 76 | { 77 | state = action_client_right.getState(); 78 | //ROS_INFO("Action state: %s",state.toString().c_str()); 79 | } 80 | 81 | if(state == actionlib::SimpleClientGoalState::SUCCEEDED) 82 | { 83 | if (action_client_right.getResult()->goal_reached) 84 | { 85 | ROS_INFO("Arm reached home position."); 86 | transition = "goal reached"; 87 | } 88 | else 89 | { 90 | ROS_WARN("Could not move arm to home position."); 91 | transition = "goal not reached"; 92 | } 93 | } 94 | else 95 | { 96 | if(state == actionlib::SimpleClientGoalState::PREEMPTED) 97 | { 98 | ROS_WARN("Could not move arm to home position. Action preempted."); 99 | transition = "goal not reached"; 100 | } 101 | else 102 | { 103 | ROS_INFO("Home position not reached. Unhandled action state: %s",state.toString().c_str()); 104 | transition = "error"; 105 | } 106 | } 107 | 108 | return transition; 109 | } 110 | -------------------------------------------------------------------------------- /application_control/src/ReadyState.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014,Umeå University 5 | * Copyright (c) 2014,DLO, Wageningen University & Research Center. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Open Source Robotics Foundation nor 19 | * the names of its contributors may be used to endorse or promote 20 | * products derived from this software without specific prior 21 | * written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #include "ReadyState.h" 38 | 39 | 40 | /* 41 | * Starts state machine. 42 | * 43 | * From the command line it can be manually triggered by: 44 | * 45 | * $ rostopic pub /state_machine_start std_tring "start state machine" 46 | * 47 | */ 48 | void ReadyState::startCallback(const std_msgs::String::ConstPtr& msg) 49 | { 50 | ROS_INFO("Recieved message : %s", msg->data.c_str()); 51 | if (msg->data == "start state machine") 52 | { 53 | ROS_INFO("Starting main program."); 54 | transition = "start"; 55 | } 56 | else 57 | { 58 | ROS_INFO("Main program not ready yet."); 59 | transition = "not ready"; 60 | } 61 | } 62 | 63 | 64 | ReadyState::ReadyState() 65 | { 66 | transition = "not ready"; 67 | start_subscriber = node_handle.subscribe("state_machine_start", 10, &ReadyState::startCallback, this); 68 | } 69 | 70 | 71 | std::string ReadyState::execute(std::map * data) 72 | { 73 | transition = "not ready"; 74 | ros::Rate rate(1.0); 75 | rate.sleep(); 76 | return transition; 77 | } 78 | -------------------------------------------------------------------------------- /application_control/src/ScanPlantState.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014,Umeå University 5 | * Copyright (c) 2014,DLO, Wageningen University & Research Center. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Open Source Robotics Foundation nor 19 | * the names of its contributors may be used to endorse or promote 20 | * products derived from this software without specific prior 21 | * written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #include "ScanPlantState.h" 38 | 39 | 40 | 41 | 42 | 43 | /* 44 | * Service client function to request to endpoint state of the robot. 45 | * This information is needed to save the pose that corresponds to 46 | * the view with the most recognized pepper. 47 | */ 48 | geometry_msgs::PoseStamped ScanPlantState::requestEndPointState() 49 | { 50 | std::string request_message = "trigger"; 51 | robot_control::request_endpointstate service; 52 | service.request.request_message = request_message; 53 | if (robot_endpointstate_client.call(service)) 54 | { 55 | return service.response.pose; 56 | } 57 | else 58 | { 59 | ROS_INFO("Could not request endpoint state of the robot."); 60 | } 61 | } 62 | 63 | 64 | 65 | 66 | /* 67 | * Callback function for updating the current pose of the endpoint of the robot's arm. 68 | */ 69 | void ScanPlantState::EndpointStateCallback_Baxter(const baxter_core_msgs::EndpointState::ConstPtr& endpoint_msg) 70 | { 71 | robot_endpoint_pose.header.frame_id = "base"; 72 | robot_endpoint_pose.header.stamp = ros::Time::now(); 73 | 74 | robot_endpoint_pose.pose.position.x = endpoint_msg->pose.position.x; 75 | robot_endpoint_pose.pose.position.y = endpoint_msg->pose.position.y; 76 | robot_endpoint_pose.pose.position.z = endpoint_msg->pose.position.z; 77 | 78 | robot_endpoint_pose.pose.orientation.x = endpoint_msg->pose.orientation.x; 79 | robot_endpoint_pose.pose.orientation.y = endpoint_msg->pose.orientation.y; 80 | robot_endpoint_pose.pose.orientation.z = endpoint_msg->pose.orientation.z; 81 | robot_endpoint_pose.pose.orientation.w = endpoint_msg->pose.orientation.w; 82 | } 83 | 84 | 85 | 86 | 87 | /* 88 | * Requests an image from Image Aquisition node. 89 | * Requests blob size in that image from Fruit detection node. 90 | * If area is larger than any before, store it with current end-effector pose. 91 | */ 92 | bool ScanPlantState::detectFruit() 93 | { 94 | // Request image from node that is responsible for image acquisition. 95 | HalconCpp::HImage halcon_image; 96 | std::string trigger_message = "trigger"; 97 | image_acquisition::request_image image_service; 98 | image_service.request.request_message = trigger_message; 99 | 100 | if (image_client_rgb.call(image_service)) 101 | { 102 | // Get fruit detection node's features from acquired image. 103 | fruit_detection::request_features features_service; 104 | features_service.request.image = image_service.response.image; 105 | 106 | if(request_features_client.call(features_service)) 107 | { 108 | // Add fruit to global list. 109 | detected_fruit fruit; 110 | fruit.area = features_service.response.pepper_blob_size; 111 | fruit.pose = robot_endpoint_pose; 112 | // detected_fruits.push_back(fruit); 113 | 114 | ROS_INFO("Detected ripe fruit area : %i", fruit.area); 115 | 116 | // Check if detected pepper in current image is larger than in any other image. 117 | // If so, store it globally with its corresponding pose. 118 | if(fruit.area > best_fruit.area) 119 | { 120 | best_fruit.area = fruit.area; 121 | best_fruit.pose = robot_endpoint_pose; 122 | } 123 | } 124 | else 125 | { 126 | ROS_INFO("Could not call fruit detection feature service."); 127 | } 128 | } 129 | else 130 | { 131 | ROS_INFO("Could not call image aqcuisition service."); 132 | } 133 | } 134 | 135 | 136 | 137 | 138 | std::string ScanPlantState::goToScanWaypoint(XmlRpc::XmlRpcValue waypoint) 139 | { 140 | // Create the action client for arm movement. True causes the client to spin its own thread. 141 | actionlib::SimpleActionClient action_client_right("robot_control_right_arm", true); 142 | 143 | ROS_INFO("Waiting for action server for moving rigth Baxter arm to start."); 144 | action_client_right.waitForServer(); //will wait for infinite time 145 | ROS_INFO("Action server for moving rigth Baxter arm is started."); 146 | 147 | robot_control::MoveArmGoal goal_message; 148 | goal_message.goal_pose.header.stamp = ros::Time::now(); 149 | goal_message.goal_pose.header.frame_id = "base"; 150 | goal_message.goal_pose.pose.position.x = waypoint[0]; 151 | goal_message.goal_pose.pose.position.y = waypoint[1]; 152 | goal_message.goal_pose.pose.position.z = waypoint[2]; 153 | goal_message.goal_pose.pose.orientation.x = waypoint[3]; 154 | goal_message.goal_pose.pose.orientation.y = waypoint[4]; 155 | goal_message.goal_pose.pose.orientation.z = waypoint[5]; 156 | goal_message.goal_pose.pose.orientation.w = waypoint[6]; 157 | 158 | ROS_INFO("Sending goal for scan waypoint."); 159 | action_client_right.sendGoal(goal_message); 160 | 161 | // Loop while waiting for the goal to execute. 162 | // During this loop, the scanning of the crop is performed. 163 | actionlib::SimpleClientGoalState state = actionlib::SimpleClientGoalState::ACTIVE; 164 | while(state == actionlib::SimpleClientGoalState::PENDING || state == actionlib::SimpleClientGoalState::ACTIVE) 165 | { 166 | state = action_client_right.getState(); 167 | detectFruit(); 168 | } 169 | 170 | std:string result; 171 | if(state == actionlib::SimpleClientGoalState::SUCCEEDED) 172 | { 173 | if (action_client_right.getResult()->goal_reached) 174 | { 175 | ROS_INFO("Arm reached position."); 176 | result = "goal reached"; 177 | } 178 | else 179 | { 180 | ROS_WARN("Could not move arm to position."); 181 | result = "goal not reached"; 182 | } 183 | } 184 | else 185 | { 186 | if(state == actionlib::SimpleClientGoalState::PREEMPTED) 187 | { 188 | ROS_WARN("Could not move arm to position. Action preempted."); 189 | result = "goal not reached"; 190 | } 191 | else 192 | { 193 | ROS_INFO("Goal not reached. Unhandled action state: %s",state.toString().c_str()); 194 | result = "error"; 195 | } 196 | } 197 | return result; 198 | } 199 | 200 | 201 | 202 | 203 | 204 | /* 205 | * Set starting point for visual servo control in next state. 206 | * If a pepper was detected that is greater than a threshold, 207 | * then use that as visual start pose. Otherwise use hardcoded pose 208 | * from launchfile. 209 | */ 210 | void ScanPlantState::setVisualServoStartingPose(std::map * data) 211 | { 212 | 213 | ROS_INFO("Largest detected area: %i", best_fruit.area); 214 | if(best_fruit.area > 0) 215 | { 216 | ROS_INFO("Visual servo start pose from scanning."); 217 | 218 | // Use the best pose found pose during scanning. 219 | // No transformation is needed because the gripper 220 | // pose is already in the /base frame. 221 | geometry_msgs::PoseStamped visual_servo_start_pose; 222 | 223 | visual_servo_start_pose.header.frame_id = "base"; 224 | visual_servo_start_pose.pose.position.x = best_fruit.pose.pose.position.x; 225 | visual_servo_start_pose.pose.position.y = best_fruit.pose.pose.position.y; 226 | visual_servo_start_pose.pose.position.z = best_fruit.pose.pose.position.z; 227 | visual_servo_start_pose.pose.orientation.x = best_fruit.pose.pose.orientation.x; 228 | visual_servo_start_pose.pose.orientation.y = best_fruit.pose.pose.orientation.y; 229 | visual_servo_start_pose.pose.orientation.z = best_fruit.pose.pose.orientation.z; 230 | visual_servo_start_pose.pose.orientation.w = best_fruit.pose.pose.orientation.w; 231 | 232 | (*data)["visual_servo_start_pose"] = visual_servo_start_pose; 233 | } 234 | else 235 | { 236 | ROS_INFO("Visual servo start pose from launch file."); 237 | 238 | // Hardcode visual servo starting pose from launch file. 239 | XmlRpc::XmlRpcValue servo_start_pose; 240 | node_handle.getParam("visual_servo_start_pose", servo_start_pose); 241 | geometry_msgs::PoseStamped visual_servo_start_pose; 242 | 243 | visual_servo_start_pose.header.frame_id = "base"; 244 | visual_servo_start_pose.pose.position.x = servo_start_pose[0]; 245 | visual_servo_start_pose.pose.position.y = servo_start_pose[1]; 246 | visual_servo_start_pose.pose.position.z = servo_start_pose[2]; 247 | visual_servo_start_pose.pose.orientation.x = servo_start_pose[3]; 248 | visual_servo_start_pose.pose.orientation.y = servo_start_pose[4]; 249 | visual_servo_start_pose.pose.orientation.z = servo_start_pose[5]; 250 | visual_servo_start_pose.pose.orientation.w = servo_start_pose[6]; 251 | 252 | (*data)["visual_servo_start_pose"] = visual_servo_start_pose; 253 | } 254 | } 255 | 256 | 257 | 258 | 259 | 260 | 261 | ScanPlantState::ScanPlantState() 262 | { 263 | // Initially the goal is not reached. 264 | transition = "goal not reached"; 265 | 266 | // Get home position from launch file. 267 | node_handle.getParam("scan_waypoint_1", scan_waypoint_1); 268 | node_handle.getParam("scan_waypoint_2", scan_waypoint_2); 269 | node_handle.getParam("scan_waypoint_3", scan_waypoint_3); 270 | node_handle.getParam("scan_waypoint_4", scan_waypoint_4); 271 | 272 | // Client for requesting an RGB image. 273 | image_client_rgb = node_handle.serviceClient("/image_acquisition_node/request_image_rgb"); 274 | 275 | // Client for requesting detected fruit features from image. 276 | request_features_client = node_handle.serviceClient("/fruit_detection_node/request_features"); 277 | 278 | // Client for requesting the endpoint state of the robot. Used to track which pose detects the most pepper. 279 | robot_endpointstate_client = node_handle.serviceClient("/request_right_endpoint_state"); 280 | 281 | // Callback for continously updating the endpoint. 282 | robot_endpoint_state_subscriber = node_handle.subscribe("/robot/limb/right/endpoint_state", 1000, &ScanPlantState::EndpointStateCallback_Baxter, this); 283 | } 284 | 285 | 286 | 287 | 288 | 289 | 290 | std::string ScanPlantState::execute(std::map * data) 291 | { 292 | // Clear previously detected fruits. 293 | detected_fruits.clear(); 294 | best_fruit.area = 0; 295 | 296 | while(true) 297 | { 298 | detectFruit(); 299 | } 300 | 301 | // Go to all scan waypoints. 302 | //goToScanWaypoint(scan_waypoint_1); 303 | //goToScanWaypoint(scan_waypoint_2); 304 | //goToScanWaypoint(scan_waypoint_3); 305 | //goToScanWaypoint(scan_waypoint_4); 306 | 307 | // For the next state, it is required that starting point is defined. 308 | setVisualServoStartingPose(data); 309 | 310 | return "goal reached"; 311 | } 312 | -------------------------------------------------------------------------------- /application_control/srv/PauseRequest.srv: -------------------------------------------------------------------------------- 1 | # Request for the state machine to pause the system (stop the arm and so on) 2 | bool pause 3 | --- 4 | bool succeded 5 | -------------------------------------------------------------------------------- /application_control/srv/request_camera_parameters.srv: -------------------------------------------------------------------------------- 1 | string request_message 2 | --- 3 | float64 f 4 | float64 k1 5 | float64 k2 6 | float64 k3 7 | float64 P1 8 | float64 P2 9 | float64 Sx 10 | float64 Sy 11 | float64 Cx 12 | float64 Cy 13 | float64 Iw 14 | float64 Ih -------------------------------------------------------------------------------- /application_control/transitions/transitions.dot: -------------------------------------------------------------------------------- 1 | digraph{ 2 | ColdBootState->ColdBootState [ label="not ready" ]; 3 | ColdBootState->ReadyState [ label="ready" ]; 4 | ReadyState->ReadyState [ label="not ready" ]; 5 | ReadyState->ScanPlantState [ label="start" ]; 6 | MoveToHomeState->ColdBootState [ label="error" ]; 7 | MoveToHomeState->ScanPlantState [ label="goal reached" ]; 8 | MoveToHomeState->ColdBootState [ label="goal not reached" ]; 9 | ScanPlantState->VisualServoState [ label="goal reached" ]; 10 | ScanPlantState->ColdBootState [ label="goal not reached" ]; 11 | ScanPlantState->ColdBootState [ label="error" ]; 12 | VisualServoState->ReadyState [ label="goal reached" ]; 13 | VisualServoState->ColdBootState [ label="error" ]; 14 | VisualServoState->ColdBootState [ label="goal not reached" ]; 15 | } 16 | -------------------------------------------------------------------------------- /application_control/transitions/transitions.yaml: -------------------------------------------------------------------------------- 1 | - start_state: ColdBootState 2 | - state: 3 | state_label: ColdBootState 4 | descriptive_name: Initializing all nodes and programs. 5 | transitions: 6 | not ready : ColdBootState 7 | ready : ReadyState 8 | - state: 9 | state_label: ReadyState 10 | descriptive_name: Starting state where all systems are ready to go. 11 | transitions: 12 | not ready : ReadyState 13 | start : ScanPlantState 14 | - state: 15 | state_label: MoveToHomeState 16 | descriptive_name: Moves robot to home position. 17 | transitions: 18 | goal reached : ScanPlantState 19 | goal not reached : ColdBootState 20 | error : ColdBootState 21 | - state: 22 | state_label: ScanPlantState 23 | descriptive_name: Moves robot along the plant to scan it. 24 | transitions: 25 | goal reached : VisualServoState 26 | goal not reached : ColdBootState 27 | error : ColdBootState 28 | - state: 29 | state_label: VisualServoState 30 | descriptive_name: Moves robot towards the fruit. 31 | transitions: 32 | goal reached : ReadyState 33 | goal not reached : ColdBootState 34 | error : ColdBootState 35 | -------------------------------------------------------------------------------- /fruit_detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(fruit_detection) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | tf 9 | roscpp 10 | rospy 11 | std_msgs 12 | message_generation 13 | geometry_msgs 14 | sensor_msgs 15 | image_transport 16 | cv_bridge 17 | ) 18 | 19 | find_package(Boost REQUIRED COMPONENTS thread) 20 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 39 | ## pulled in transitively but can be declared for certainty nonetheless: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a run_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | add_service_files( 61 | FILES 62 | request_features.srv 63 | ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | generate_messages( 74 | DEPENDENCIES 75 | std_msgs 76 | sensor_msgs 77 | ) 78 | 79 | ################################### 80 | ## catkin specific configuration ## 81 | ################################### 82 | ## The catkin_package macro generates cmake config files for your package 83 | ## Declare things to be passed to dependent projects 84 | ## INCLUDE_DIRS: uncomment this if you package contains header files 85 | ## LIBRARIES: libraries you create in this project that dependent projects also need 86 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 87 | ## DEPENDS: system dependencies of this project that dependent projects also need 88 | catkin_package( 89 | # INCLUDE_DIRS include 90 | # LIBRARIES fruit_detection 91 | CATKIN_DEPENDS roscpp rospy std_msgs 92 | # DEPENDS system_lib 93 | ) 94 | 95 | ########### 96 | ## Build ## 97 | ########### 98 | 99 | ## Specify additional locations of header files 100 | ## Your package locations should be listed before other locations 101 | include_directories($ENV{HALCONROOT}/include) 102 | include_directories($ENV{HALCONROOT}/include/halconcpp) 103 | include_directories($ENV{HALCONROOT}/include/hdevengine) 104 | include_directories(${catkin_INCLUDE_DIRS}) 105 | include_directories(include) 106 | 107 | ## Declare a cpp library 108 | # add_library(fruit_detection 109 | # src/${PROJECT_NAME}/fruit_detection.cpp 110 | # ) 111 | 112 | ## Declare a cpp executable 113 | add_executable(fruit_detection_node src/fruit_detection_node.cpp) 114 | 115 | ## Add cmake target dependencies of the executable/library 116 | ## as an example, message headers may need to be generated before nodes 117 | add_dependencies(fruit_detection_node fruit_detection_node_generate_messages_cpp) 118 | 119 | # set the path to the library folder 120 | link_directories(/opt/halcon/lib/x64-linux) 121 | 122 | ## Specify libraries to link a library or executable target against 123 | target_link_libraries(fruit_detection_node ${catkin_LIBRARIES}) 124 | target_link_libraries(fruit_detection_node $ENV{HALCONROOT}/lib/$ENV{HALCONARCH}/libhdevenginecpp.so ${catkin_LIBRARIES}) 125 | target_link_libraries(fruit_detection_node $ENV{HALCONROOT}/lib/$ENV{HALCONARCH}/libhalconcpp.so ${catkin_LIBRARIES}) 126 | target_link_libraries(fruit_detection_node $ENV{HALCONROOT}/lib/$ENV{HALCONARCH}/libhalcon.so ${catkin_LIBRARIES}) 127 | target_link_libraries(fruit_detection_node ${VISP_LIBRARIES}) 128 | target_link_libraries(fruit_detection_node ${Boost_LIBRARIES} ${catkin_LIBRARIES}) 129 | 130 | find_package(OpenCV REQUIRED) 131 | include_directories(${OpenCV_INCLUDE_DIRS}) 132 | target_link_libraries(fruit_detection_node ${OpenCV_LIBRARIES}) 133 | 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS fruit_detection fruit_detection_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_fruit_detection.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /fruit_detection/halcon_external_procedures/find_pepper_features.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | dev_update_off() 21 | 22 | 23 | * Decompose RGB in separate channels and transform this to Hue, Saturation and Intesity channels. 24 | *decompose3 (Image, ImageR, ImageG, ImageB) 25 | *trans_from_rgb (ImageR, ImageG, ImageB, ImageResultH, ImageResultS, ImageResultI, 'hsv') 26 | * Remove noise by smoothing the Hue channel. 27 | *gauss_image (ImageResultH, ImageGauss, 9) 28 | * Segment region in color range to get all red pixels in a range. 29 | *threshold (ImageGauss, Regions, HueLowerThreshold, HueUpperThreshold) 30 | 31 | 32 | * New segmentation in cielab. 33 | *decompose3 (Image, ImageR, ImageG, ImageB) 34 | *trans_from_rgb (ImageR, ImageG, ImageB, ImageResult1, ImageResult2, ImageResult3, 'yuv') 35 | *threshold (ImageResult2, Regions, HueLowerThreshold, HueUpperThreshold) 36 | 37 | * Pepper Demo 38 | decompose3 (Image, ImageR, ImageG, ImageB) 39 | trans_from_rgb (ImageR, ImageG, ImageB, ImageResult1, ImageResult2, ImageResult3, 'hsv') 40 | threshold (ImageResult1, Regions1, 0, 36) 41 | threshold (ImageResult2, Regions2, 66, 136) 42 | intersection(Regions1, Regions2, Regions) 43 | 44 | 45 | 46 | * Remove further small noise regions. 47 | erosion_circle (Regions, RegionErosion, 2) 48 | dilation_circle (RegionErosion, RegionDilation, 4) 49 | 50 | * Find connected components in the segmented regions. 51 | connection (RegionDilation, ConnectedRegions) 52 | 53 | count_obj (ConnectedRegions, NumberOfRegions) 54 | if(NumberOfRegions > 0) 55 | area_center (ConnectedRegions, Area, Row, Column) 56 | index_largest := -1 57 | area_largest := -1 58 | for i := 1 to NumberOfRegions by 1 59 | 60 | if(Area[i-1] > area_largest) 61 | index_largest := i 62 | area_largest := Area[i-1] 63 | endif 64 | 65 | endfor 66 | select_obj(ConnectedRegions, ObjectSelected, index_largest) 67 | area_center (ObjectSelected, Area_Object, raw_cog_y, raw_cog_x) 68 | 69 | dev_get_window (CurrentWindowID) 70 | get_image_size (Image, Width, Height) 71 | if (CurrentWindowID != -1) 72 | dev_resize_window_fit_image (Image, 0, 0, -1, -1) 73 | dev_display (Image) 74 | if(area_largest > 1500) 75 | dev_set_color ('red') 76 | dev_display (ObjectSelected) 77 | dev_set_color ('blue') 78 | gen_circle (Circle, raw_cog_y, raw_cog_x, 5) 79 | dev_display (Circle) 80 | dev_set_color ('white') 81 | gen_arrow_contour_xld (Arrow, 240, 320, raw_cog_y, raw_cog_x,5, 5) 82 | dev_display (Arrow) 83 | endif 84 | dev_update_off() 85 | else 86 | dev_open_window (0, 0, Width, Height, 'black', WindowHandle) 87 | dev_resize_window_fit_image (Image, 0, 0, -1, -1) 88 | dev_display (Image) 89 | if(area_largest > 1500) 90 | dev_set_color ('red') 91 | dev_display (ObjectSelected) 92 | dev_set_color ('blue') 93 | gen_circle (Circle, raw_cog_y, raw_cog_x, 5) 94 | dev_display (Circle) 95 | dev_set_color ('white') 96 | gen_arrow_contour_xld (Arrow, 240, 320, raw_cog_y, raw_cog_x,5, 5) 97 | dev_display (Arrow) 98 | endif 99 | 100 | dev_update_off() 101 | endif 102 | 103 | * y has to be flipped due to the inversed coordinate system of Halcon. 104 | cog_y := Height - raw_cog_y 105 | cog_x := raw_cog_x 106 | size := area_largest 107 | else 108 | cog_y := -1 109 | cog_x := -1 110 | area_largest := -1 111 | endif 112 | 113 | return () 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | -------------------------------------------------------------------------------- /fruit_detection/include/fruit_detection_node.h: -------------------------------------------------------------------------------- 1 | #ifndef FRUIT_DETECTION_NODE_H 2 | #define FRUIT_DETECTION_NODE_H 3 | 4 | 5 | //Include openCV first, otherwise it does not compile. 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | 12 | #include "HalconCpp.h" 13 | #include "HDevEngineCpp.h" 14 | #include "window_handling.h" 15 | 16 | #include "ros/ros.h" 17 | #include "sensor_msgs/Image.h" 18 | #include 19 | 20 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 21 | The global variables in this frame are used in the HDevEngine. 22 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 23 | HDevEngineCpp::HDevEngine hdevengine; 24 | HDevEngineCpp::HDevProgram hdev_script; 25 | HDevEngineCpp::HDevProgramCall script_results; 26 | 27 | bool halcon_script_initialized; 28 | 29 | std::string fruit_detection_halcon_script_path; 30 | std::string fruit_detection_halcon_external_procedures_path; 31 | 32 | std::string find_pepper_features_function; 33 | 34 | int hue_lower_threshold; 35 | int hue_upper_threshold; 36 | 37 | HTuple cog_x_tuple; 38 | HTuple cog_y_tuple; 39 | HTuple size_tuple; 40 | 41 | double pepper_blob_cog_x; 42 | double pepper_blob_cog_y; 43 | double pepper_blob_size; 44 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 45 | 46 | 47 | 48 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 49 | Global publishers and Subscribers of the ROS node 50 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 51 | ros::ServiceServer pepper_features_output_service; 52 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 53 | 54 | 55 | 56 | 57 | 58 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 59 | All functions within this ros node. 60 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 61 | HalconCpp::HImage rosImage2Halcon(sensor_msgs::Image &ros_image); 62 | bool getPepperFeatures(fruit_detection::request_features::Request &req, fruit_detection::request_features::Response &res); 63 | void intializeHalconProcedures(); 64 | bool runHalconScript(); 65 | void initializeHalconScript(); 66 | int main(int argc, char **argv); 67 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 68 | #endif 69 | -------------------------------------------------------------------------------- /fruit_detection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fruit_detection 4 | 0.0.0 5 | The fruit_detection package 6 | 7 | 8 | 9 | 10 | ruud 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /fruit_detection/srv/request_features.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/Image image 2 | --- 3 | float64 pepper_blob_center_of_gravity_x 4 | float64 pepper_blob_center_of_gravity_y 5 | float64 pepper_blob_size -------------------------------------------------------------------------------- /image_acquisition/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(image_acquisition) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | tf 9 | roscpp 10 | rospy 11 | std_msgs 12 | message_generation 13 | geometry_msgs 14 | image_transport 15 | cv_bridge 16 | sensor_msgs 17 | ) 18 | 19 | 20 | 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | ## Uncomment this if the package has a setup.py. This macro ensures 26 | ## modules and global scripts declared therein get installed 27 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 28 | # catkin_python_setup() 29 | 30 | ################################################ 31 | ## Declare ROS messages, services and actions ## 32 | ################################################ 33 | 34 | ## To declare and build messages, services or actions from within this 35 | ## package, follow these steps: 36 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 37 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 38 | ## * In the file package.xml: 39 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 41 | ## pulled in transitively but can be declared for certainty nonetheless: 42 | ## * add a build_depend tag for "message_generation" 43 | ## * add a run_depend tag for "message_runtime" 44 | ## * In this file (CMakeLists.txt): 45 | ## * add "message_generation" and every package in MSG_DEP_SET to 46 | ## find_package(catkin REQUIRED COMPONENTS ...) 47 | ## * add "message_runtime" and every package in MSG_DEP_SET to 48 | ## catkin_package(CATKIN_DEPENDS ...) 49 | ## * uncomment the add_*_files sections below as needed 50 | ## and list every .msg/.srv/.action file to be processed 51 | ## * uncomment the generate_messages entry below 52 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 53 | 54 | ## Generate messages in the 'msg' folder 55 | add_message_files( 56 | FILES 57 | ) 58 | 59 | ## Generate services in the 'srv' folder 60 | add_service_files( 61 | FILES 62 | request_camera_parameters.srv 63 | request_image.srv 64 | ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | generate_messages( 75 | DEPENDENCIES 76 | std_msgs 77 | sensor_msgs 78 | ) 79 | 80 | ################################### 81 | ## catkin specific configuration ## 82 | ################################### 83 | ## The catkin_package macro generates cmake config files for your package 84 | ## Declare things to be passed to dependent projects 85 | ## INCLUDE_DIRS: uncomment this if you package contains header files 86 | ## LIBRARIES: libraries you create in this project that dependent projects also need 87 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 88 | ## DEPENDS: system dependencies of this project that dependent projects also need 89 | catkin_package( 90 | # INCLUDE_DIRS include 91 | # LIBRARIES hdev_engine 92 | CATKIN_DEPENDS roscpp rospy std_msgs 93 | # DEPENDS system_lib 94 | ) 95 | 96 | ########### 97 | ## Build ## 98 | ########### 99 | 100 | ## Specify additional locations of header files 101 | ## Your package locations should be listed before other locations 102 | include_directories($ENV{HALCONROOT}/include) 103 | include_directories($ENV{HALCONROOT}/include/halconcpp) 104 | include_directories($ENV{HALCONROOT}/include/hdevengine) 105 | #include_directories($ENV{HALCONROOT}/examples/hdevengine/cpp/source) 106 | include_directories(${catkin_INCLUDE_DIRS}) 107 | include_directories(include) 108 | 109 | 110 | ## Declare a cpp library 111 | # add_library(hdev_engine 112 | # src/${PROJECT_NAME}/hdev_engine.cpp 113 | # ) 114 | 115 | ## Declare a cpp executable 116 | add_executable(image_acquisition_node src/image_acquisition_node.cpp) 117 | 118 | ## Add cmake target dependencies of the executable/library 119 | ## as an example, message headers may need to be generated before nodes 120 | add_dependencies(image_acquisition_node image_acquisition_node_generate_messages_cpp) 121 | 122 | # set the path to the library folder 123 | link_directories(/opt/halcon/lib/x64-linux) 124 | 125 | 126 | ## Specify libraries to link a library or executable target against 127 | target_link_libraries(image_acquisition_node ${catkin_LIBRARIES}) 128 | target_link_libraries(image_acquisition_node $ENV{HALCONROOT}/lib/$ENV{HALCONARCH}/libhdevenginecpp.so ${catkin_LIBRARIES}) 129 | target_link_libraries(image_acquisition_node $ENV{HALCONROOT}/lib/$ENV{HALCONARCH}/libhalconcpp.so ${catkin_LIBRARIES}) 130 | target_link_libraries(image_acquisition_node $ENV{HALCONROOT}/lib/$ENV{HALCONARCH}/libhalcon.so ${catkin_LIBRARIES} ) 131 | 132 | 133 | 134 | find_package(OpenCV REQUIRED) 135 | include_directories(${OpenCV_INCLUDE_DIRS}) 136 | target_link_libraries(image_acquisition_node ${OpenCV_LIBRARIES}) 137 | 138 | 139 | find_package(Boost REQUIRED COMPONENTS thread) 140 | include_directories(${Boost_INCLUDE_DIRS}) 141 | target_link_libraries(image_acquisition_node ${Boost_LIBRARIES}) 142 | 143 | 144 | ############# 145 | ## Install ## 146 | ############# 147 | 148 | # all install targets should use catkin DESTINATION variables 149 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 150 | 151 | ## Mark executable scripts (Python etc.) for installation 152 | ## in contrast to setup.py, you can choose the destination 153 | # install(PROGRAMS 154 | # scripts/my_python_script 155 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 156 | # ) 157 | 158 | ## Mark executables and/or libraries for installation 159 | # install(TARGETS hdev_engine hdev_engine_node 160 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 161 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 162 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark cpp header files for installation 166 | # install(DIRECTORY include/${PROJECT_NAME}/ 167 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 168 | # FILES_MATCHING PATTERN "*.h" 169 | # PATTERN ".svn" EXCLUDE 170 | # ) 171 | 172 | ## Mark other files for installation (e.g. launch and bag files, etc.) 173 | # install(FILES 174 | # # myfile1 175 | # # myfile2 176 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 177 | # ) 178 | 179 | ############# 180 | ## Testing ## 181 | ############# 182 | 183 | ## Add gtest based cpp test target and link libraries 184 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_hdev_engine.cpp) 185 | # if(TARGET ${PROJECT_NAME}-test) 186 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 187 | # endif() 188 | 189 | ## Add folders to be run by python nosetests 190 | # catkin_add_nosetests(test) 191 | -------------------------------------------------------------------------------- /image_acquisition/camera_parameters/calculate_camera_parameter_tis_camera.hdev: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | dev_close_window () 7 | dev_open_window (0, 0, 768, 576, 'black', WindowID) 8 | CalTabDescrName := '/home/ruud/Dropbox/halcon_scripts/calibration_model_125.descr' 9 | 10 | 11 | * To initialize the initial rough camera parameter (which are refined later in this script), 12 | * a calibration plate is searched and processed here. 13 | read_image(Image, '/home/ruud/Dropbox/halcon_scripts/tis_calibration_images/image02.png') 14 | find_caltab(Image, Caltab, '/home/ruud/Dropbox/halcon_scripts/calibration_model_125.descr', 3, 112, 5) 15 | * find calibration marks and start pose 16 | find_marks_and_pose(Image, Caltab, '/home/ruud/Dropbox/halcon_scripts/calibration_model_125.descr' , \ 17 | [0.008, 0.0, 0.0000022, 0.0000022, 320, 240, 640, 480], \ 18 | 128, 10, 18, 0.9, 15.0, 100.0, RCoord, CCoord, StartPose) 19 | 20 | StartCamParam := [0.025,0.0,0.0,0.0,0.0,0.0,0.000011,0.000011,384.0,288.0,640,480] 21 | create_calib_data ('calibration_object', 1, 1, CalibDataID) 22 | set_calib_data_calib_object (CalibDataID, 0, CalTabDescrName) 23 | set_calib_data_cam_param (CalibDataID, 0, 'area_scan_polynomial', StartCamParam) 24 | 25 | for I := 0 to 200 by 1 26 | try 27 | * Read next image 28 | read_image (Image, '/home/ruud/Dropbox/halcon_scripts/tis_calibration_images/image'+ I$'02d') 29 | dev_display (Image) 30 | find_calib_object (Image, CalibDataID, 0, 0, I, [], []) 31 | * Display the caltab and the marks found 32 | get_calib_data_observ_contours (CalTab, CalibDataID, 'last_caltab', 0, 0, I) 33 | get_calib_data_observ_contours (Marks, CalibDataID, 'marks', 0, 0, I) 34 | get_calib_data_observ_points (CalibDataID, 0, 0, I, Row, Column, Index, Pose) 35 | gen_circle (Circles, Row, Column, Row * 0.0 + 2.0) 36 | dev_set_color ('green') 37 | dev_set_line_width (3) 38 | dev_display (CalTab) 39 | dev_set_color ('magenta') 40 | dev_set_line_width (1) 41 | dev_display (Marks) 42 | dev_set_line_width (3) 43 | set_system ('flush_graphic', 'true') 44 | dev_display (Circles) 45 | set_system ('flush_graphic', 'false') 46 | catch (Exception) 47 | endtry 48 | endfor 49 | 50 | 51 | 52 | * *** C A M E R A C A L I B R A T I O N *** 53 | calibrate_cameras (CalibDataID, Errors) 54 | get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam) 55 | dev_set_color ('green') 56 | dev_set_line_width (2) 57 | 58 | for I := 0 to 200 by 1 59 | try 60 | read_image (Image, '/home/ruud/Dropbox/halcon_scripts/tis_calibration_images/image'+ I$'02d') 61 | dev_display (Image) 62 | get_calib_data (CalibDataID, 'calib_obj_pose', [0,I], 'pose', FinalPose) 63 | * Display of the calibration table 64 | disp_caltab (WindowID, CalTabDescrName, CamParam, FinalPose, 1) 65 | catch (Exception1) 66 | endtry 67 | endfor 68 | 69 | * Save calculated inner camera parameters into file 70 | write_cam_par (CamParam, '/home/ruud/Dropbox/halcon_scripts/tis_camera_parameters.dat') 71 | clear_calib_data (CalibDataID) 72 | dev_update_window ('on') 73 | 74 | change_radial_distortion_cam_par ('fixed', CamParam, [0.0,0.0,0.0,0.0,0.0], CamParamOut) 75 | for I := 0 to 200 by 1 76 | try 77 | read_image (Image, '/home/ruud/Dropbox/halcon_scripts/tis_calibration_images/image'+ I$'02d') 78 | change_radial_distortion_image (Image, Image, ImageRectified, CamParam, CamParamOut) 79 | dev_display (ImageRectified) 80 | stop() 81 | catch (Exception2) 82 | endtry 83 | endfor 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /image_acquisition/camera_parameters/calibration_model_125.descr: -------------------------------------------------------------------------------- 1 | # Plate Description Version 2 2 | # HALCON Version 11.0 -- Fri Sep 12 11:13:39 2014 3 | # Description of the standard calibration plate 4 | # used for the camera calibration in HALCON 5 | # (generated by gen_caltab) 6 | # 7 | # 8 | 9 | # 7 rows x 7 columns 10 | # Width, height of calibration plate [meter]: 0.1, 0.1 11 | # Distance between mark centers [meter]: 0.0125 12 | 13 | # Number of marks in y-dimension (rows) 14 | r 7 15 | 16 | # Number of marks in x-dimension (columns) 17 | c 7 18 | 19 | # offset of coordinate system in z-dimension [meter] (optional): 20 | z 0 21 | 22 | # Rectangular border (rim and black frame) of calibration plate 23 | # rim of the calibration plate (min x, max y, max x, min y) [meter]: 24 | o -0.05125 0.05125 0.05125 -0.05125 25 | # outer border of the black frame (min x, max y, max x, min y) [meter]: 26 | i -0.05 0.05 0.05 -0.05 27 | # triangular corner mark given by two corner points (x,y, x,y) [meter] 28 | # (optional): 29 | t -0.05 -0.0375 -0.0375 -0.05 30 | 31 | # width of the black frame [meter]: 32 | w 0.003125 33 | 34 | # calibration marks: x y radius [meter] 35 | 36 | # calibration marks at y = -0.0375 m 37 | -0.0375 -0.0375 0.003125 38 | -0.025 -0.0375 0.003125 39 | -0.0125 -0.0375 0.003125 40 | 0 -0.0375 0.003125 41 | 0.0125 -0.0375 0.003125 42 | 0.025 -0.0375 0.003125 43 | 0.0375 -0.0375 0.003125 44 | 45 | # calibration marks at y = -0.025 m 46 | -0.0375 -0.025 0.003125 47 | -0.025 -0.025 0.003125 48 | -0.0125 -0.025 0.003125 49 | 0 -0.025 0.003125 50 | 0.0125 -0.025 0.003125 51 | 0.025 -0.025 0.003125 52 | 0.0375 -0.025 0.003125 53 | 54 | # calibration marks at y = -0.0125 m 55 | -0.0375 -0.0125 0.003125 56 | -0.025 -0.0125 0.003125 57 | -0.0125 -0.0125 0.003125 58 | 0 -0.0125 0.003125 59 | 0.0125 -0.0125 0.003125 60 | 0.025 -0.0125 0.003125 61 | 0.0375 -0.0125 0.003125 62 | 63 | # calibration marks at y = 0 m 64 | -0.0375 0 0.003125 65 | -0.025 0 0.003125 66 | -0.0125 0 0.003125 67 | 0 0 0.003125 68 | 0.0125 0 0.003125 69 | 0.025 0 0.003125 70 | 0.0375 0 0.003125 71 | 72 | # calibration marks at y = 0.0125 m 73 | -0.0375 0.0125 0.003125 74 | -0.025 0.0125 0.003125 75 | -0.0125 0.0125 0.003125 76 | 0 0.0125 0.003125 77 | 0.0125 0.0125 0.003125 78 | 0.025 0.0125 0.003125 79 | 0.0375 0.0125 0.003125 80 | 81 | # calibration marks at y = 0.025 m 82 | -0.0375 0.025 0.003125 83 | -0.025 0.025 0.003125 84 | -0.0125 0.025 0.003125 85 | 0 0.025 0.003125 86 | 0.0125 0.025 0.003125 87 | 0.025 0.025 0.003125 88 | 0.0375 0.025 0.003125 89 | 90 | # calibration marks at y = 0.0375 m 91 | -0.0375 0.0375 0.003125 92 | -0.025 0.0375 0.003125 93 | -0.0125 0.0375 0.003125 94 | 0 0.0375 0.003125 95 | 0.0125 0.0375 0.003125 96 | 0.025 0.0375 0.003125 97 | 0.0375 0.0375 0.003125 98 | -------------------------------------------------------------------------------- /image_acquisition/camera_parameters/tis_camera_parameters.dat: -------------------------------------------------------------------------------- 1 | # 2 | # INTERNAL CAMERA PARAMETERS 3 | # 4 | 5 | # ===== generic parameter syntax ========================================== 6 | # 7 | # Syntax: ParGroup: ; 8 | # ; 9 | # 10 | # : : ; 11 | # : : ; 12 | # ; 13 | # 14 | # [ ::= BOOL|XBOOL|INT|FLOAT|DOUBLE|STRING ] 15 | # 16 | # ========================================================================= 17 | 18 | 19 | 20 | ############################################################################## 21 | # 22 | # Camera : Parameter 23 | # > Focus 24 | # > Poly1 25 | # > Poly2 26 | # > Poly3 27 | # > Poly4 28 | # > Poly5 29 | # > Sx 30 | # > Sy 31 | # > Cx 32 | # > Cy 33 | # > ImageWidth 34 | # > ImageHeight 35 | # 36 | ############################################################################## 37 | 38 | ParGroup: Camera: Parameter; 39 | "Internal camera parameters"; 40 | 41 | Focus:foc: 0.00546213886287705; 42 | DOUBLE:0.0:; 43 | "Focal length of the lens"; 44 | 45 | Poly1:poly1: 14522.3013203177; 46 | DOUBLE::; 47 | "1st polynomial distortion coefficient"; 48 | 49 | Poly2:poly2: 334976413.652045; 50 | DOUBLE::; 51 | "2nd polynomial distortion coefficient"; 52 | 53 | Poly3:poly3: 9222913439869.82; 54 | DOUBLE::; 55 | "3rd polynomial distortion coefficient"; 56 | 57 | Poly4:poly4: -240.930452950165; 58 | DOUBLE::; 59 | "4th polynomial distortion coefficient * 1000"; 60 | 61 | Poly5:poly5: 1373.27011160605; 62 | DOUBLE::; 63 | "5th polynomial distortion coefficient * 1000"; 64 | 65 | Sx:sx: 1.09363999535642e-05; 66 | DOUBLE:0.0:; 67 | "Width of a cell on the sensor"; 68 | 69 | Sy:sy: 1.1e-05; 70 | DOUBLE:0.0:; 71 | "Height of a cell on the sensor"; 72 | 73 | Cx:cx: 308.168000436854; 74 | DOUBLE::; 75 | "X-coordinate of the image center"; 76 | 77 | Cy:cy: 283.345521518823; 78 | DOUBLE::; 79 | "Y-coordinate of the image center"; 80 | 81 | ImageWidth:imgw: 640; 82 | INT:1:32768; 83 | "Width of the images"; 84 | 85 | ImageHeight:imgh: 480; 86 | INT:1:32768; 87 | "Height of the images"; 88 | 89 | 90 | # 91 | # HALCON Version 11.0 -- Wed Oct 29 15:24:08 2014 92 | # 93 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/close_frame_grabber.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | * Close the framegrabber 11 | close_framegrabber (AcqHandle) 12 | return () 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/display_grabbed_image.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | * Display image. You have to open a window yourself for use in HDevEngine. 11 | dev_get_window (CurrentWindowID) 12 | if (CurrentWindowID != -1) 13 | dev_resize_window_fit_image (Image, 0, 0, -1, -1) 14 | dev_display (Image) 15 | dev_update_off() 16 | else 17 | get_image_size (Image, Width, Height) 18 | dev_open_window (0, 0, Width, Height, 'black', WindowHandle) 19 | dev_resize_window_fit_image (Image, 0, 0, -1, -1) 20 | dev_display (Image) 21 | dev_update_off() 22 | endif 23 | return () 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/grab_image_resized.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | grab_image_async (Image, AcqHandle, -1) 14 | cfa_to_rgb (Image, RGBImage, 'bayer_gr', 'bilinear') 15 | return () 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/grab_rectified_image.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | * Grab next image and resize. Use this sequence to avoid grabbing artifacts. 15 | grab_image_start (AcqHandle, -1) 16 | grab_image_async (Image, AcqHandle, -1) 17 | grab_image_async (Image, AcqHandle, -1) 18 | cfa_to_rgb (Image, RGBImage, 'bayer_gr', 'bilinear') 19 | zoom_image_size (RGBImage, ImageResized, 640, 480, 'constant') 20 | 21 | * Undistort taken image. 22 | change_radial_distortion_cam_par ('fixed', CamParam, [0.0,0.0,0.0,0.0,0.0], CamParamOut) 23 | change_radial_distortion_image (ImageResized, ImageResized, ImageRectified, CamParam, CamParamOut) 24 | 25 | return () 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/grab_rectified_image_640_480.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | * Grab next image and resize. Use this sequence to avoid grabbing artifacts. 15 | *grab_image_start (AcqHandle, -1) 16 | *grab_image_async (Image, AcqHandle, -1) 17 | grab_image_async (Image, AcqHandle, -1) 18 | cfa_to_rgb (Image, ImageResized, 'bayer_gr', 'bilinear') 19 | 20 | * Undistort taken image. 21 | change_radial_distortion_cam_par ('fixed', CamParam, [0.0,0.0,0.0,0.0,0.0], CamParamOut) 22 | change_radial_distortion_image (ImageResized, ImageResized, ImageRectified, CamParam, CamParamOut) 23 | 24 | return () 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/grab_rectified_image_640_480_COLORSPACE.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | * Grab next image and turn it into color. 15 | grab_image_async (ImageRaw, AcqHandle, -1) 16 | cfa_to_rgb (ImageRaw, ImageColor, 'bayer_gr', 'bilinear') 17 | 18 | * Undistort taken image. 19 | change_radial_distortion_cam_par('fixed', CamParam, [0.0,0.0,0.0,0.0,0.0], CamParamOut) 20 | change_radial_distortion_image (ImageColor, ImageColor, ImageRectified, CamParam, CamParamOut) 21 | 22 | * Separate R, G and B channels 23 | decompose3 (ImageRectified, ImageR, ImageG, ImageB) 24 | 25 | * Transform to color space so we can select all the red pixels. 26 | trans_from_rgb(ImageR, ImageG, ImageB, Image1, Image2, Image3, 'cielab') 27 | 28 | * Threshold to get all the red pixels. 29 | threshold (Image2, Region, 122, 150) 30 | 31 | * erode and dilate to reduce small red pixel noise. 32 | *erosion_circle (Region, RegionErosion, 5) 33 | *dilation_circle (RegionErosion, RegionDilation, 6) 34 | 35 | * Create a new image with only the red pixels as a mask. 36 | 37 | *reduce_domain (ImageR, RegionDilation, Image) 38 | *overpaint_region (Image, RegionDilation, 0, 'fill') 39 | 40 | get_image_size (ImageRectified, Width, Height) 41 | region_to_bin (Region, Image, 60, 255, Width, Height) 42 | 43 | return () 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/grab_rectified_image_640_480_GRAY.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | try 15 | grab_image_async (ImageResized, AcqHandle, 500) 16 | 17 | change_radial_distortion_cam_par ('fixed', CamParam, [0.0,0.0,0.0,0.0,0.0], CamParamOut) 18 | change_radial_distortion_image (ImageResized, ImageResized, ImageRectifiedGray, CamParam, CamParamOut) 19 | catch (Exception) 20 | endtry 21 | return () 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/initialize_camera_tis.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | * Open framegrabber and adjust the parameters (color, exposure). 20 | open_framegrabber ('Video4Linux2', 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto', 'video1', 0, -1, AcqHandle) 21 | 22 | *set_framegrabber_param (AcqHandle, 'grab_timeout', 50) 23 | *set_framegrabber_param (AcqHandle, 'frame_interval', 0.1) 24 | 25 | 26 | set_framegrabber_param (AcqHandle, 'Exposure (Absolute)', Exposure) 27 | set_framegrabber_param (AcqHandle, 'Gain' , Gain) 28 | set_framegrabber_param (AcqHandle, 'GainR', GainR) 29 | set_framegrabber_param (AcqHandle, 'GainG', GainG) 30 | set_framegrabber_param (AcqHandle, 'GainB', GainB) 31 | 32 | * Read in camera parameters from file. 33 | read_cam_par (CameraParametersPath, CamParam) 34 | 35 | grab_image_start (AcqHandle, -1) 36 | 37 | return () 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/initialize_camera_tis_640_480.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | * Open framegrabber and adjust the parameters (color, exposure). 20 | open_framegrabber ('Video4Linux2', 640, 480, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto', 'video1', 0, -1, AcqHandle) 21 | 22 | *set_framegrabber_param (AcqHandle, 'grab_timeout', 50) 23 | *set_framegrabber_param (AcqHandle, 'frame_interval', 0.1) 24 | 25 | set_framegrabber_param (AcqHandle, 'Binning', 4) 26 | set_framegrabber_param (AcqHandle, 'Exposure (Absolute)', Exposure) 27 | set_framegrabber_param (AcqHandle, 'Gain' , Gain) 28 | set_framegrabber_param (AcqHandle, 'GainR', GainR) 29 | set_framegrabber_param (AcqHandle, 'GainG', GainG) 30 | set_framegrabber_param (AcqHandle, 'GainB', GainB) 31 | 32 | 33 | 34 | 35 | * Read in camera parameters from file. 36 | read_cam_par (CameraParametersPath, CamParam) 37 | 38 | grab_image_start (AcqHandle, -1) 39 | return () 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /image_acquisition/halcon_external_procedures/read_camera_parameters.hdvp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | read_cam_par (CameraParametersPath, CamParam) 14 | return () 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /image_acquisition/halcon_scripts/cake_test.hdev: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | *set a variable cake that can be requested in the ros node. 7 | cake_size := 23.1 8 | 9 | *read an image of a cake. 10 | read_image (Image, '../images/cake.jpg' ) 11 | 12 | *open a new window and display the cake. 13 | get_image_size (Image, Width, Height) 14 | dev_open_window (0, 0, Width, Height, 'black', WindowHandle) 15 | dev_display (Image) 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /image_acquisition/halcon_scripts/image_analysis.hdev: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | *open framegrabber and initialize camera parameters 7 | open_framegrabber ('Video4Linux2', 640, 480, 640, 480, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto', 'video1', 0, -1, AcqHandle) 8 | set_framegrabber_param (AcqHandle, 'capture_format', 'GREY') 9 | set_framegrabber_param (AcqHandle, 'Binning', 4) 10 | set_framegrabber_param (AcqHandle, 'Exposure (Absolute)', 12) 11 | set_framegrabber_param (AcqHandle, 'GainR', 0) 12 | set_framegrabber_param (AcqHandle, 'GainG', 2) 13 | set_framegrabber_param (AcqHandle, 'GainB', 2) 14 | 15 | *grab 2 images, the first one often fails after framegrabber is opened 16 | grab_image_start (AcqHandle, -1) 17 | grab_image_resized (Image, AcqHandle) 18 | grab_image_resized (Image, AcqHandle) 19 | 20 | *show image, do not forget to open window, otherwise will crash in external program. 21 | get_image_size (Image, Width, Height) 22 | dev_open_window (0, 0, Width, Height, 'black', WindowHandle) 23 | dev_display (Image) 24 | 25 | * Find critical points 26 | critical_points_sub_pix (Image, 'facet', 5, 1, RowMin, ColumnMin, RowMax, ColumnMax, RowSaddle, ColSaddle) 27 | dev_set_color ('red') 28 | dev_set_line_width (3) 29 | for i := 0 to |RowSaddle| - 1 by 1 30 | gen_cross_contour_xld (Cross, RowSaddle[i], ColSaddle[i], 25, 0.785398) 31 | dev_display (Cross) 32 | endfor 33 | 34 | 35 | *calculate image features. 36 | 37 | point_1_x := -0.1 38 | point_1_y := -0.1 39 | point_1_z := 0 40 | 41 | point_2_x := 0.1 42 | point_2_y := -0.1 43 | point_2_z := 0 44 | 45 | point_3_x := 0.1 46 | point_3_y := 0.1 47 | point_3_z := 0 48 | 49 | point_4_x := -0.1 50 | point_4_y := 0.1 51 | point_4_z := 0 52 | 53 | 54 | close_framegrabber (AcqHandle) 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | grab_image_async (Image, AcqHandle, -1) 71 | cfa_to_rgb (Image, RGBImage, 'bayer_gr', 'bilinear') 72 | return () 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /image_acquisition/halcon_scripts/image_analysis_2.hdev: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | * Open framegrabber and adjust the parameters (color, exposure) 7 | open_framegrabber ('Video4Linux2', 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto', 'video1', 0, -1, AcqHandle) 8 | set_framegrabber_param (AcqHandle, 'Exposure (Absolute)', 100) 9 | set_framegrabber_param (AcqHandle, 'Gain' , 16) 10 | set_framegrabber_param (AcqHandle, 'GainR', 10) 11 | set_framegrabber_param (AcqHandle, 'GainG', 10) 12 | set_framegrabber_param (AcqHandle, 'GainB', 24) 13 | 14 | * Grab next image and resize. Use this sequence to avoid grabbing artifacts. 15 | grab_image_start (AcqHandle, -1) 16 | grab_image_async (Image, AcqHandle, -1) 17 | grab_image_async (Image, AcqHandle, -1) 18 | cfa_to_rgb (Image, RGBImage, 'bayer_gr', 'bilinear') 19 | zoom_image_size (RGBImage, ImageResized, 640, 480, 'constant') 20 | 21 | * Read camera parameters and undistort taken image. 22 | read_cam_par ('/home/ruud/Dropbox/halcon_scripts/tis_camera_parameters.dat', CamParam) 23 | change_radial_distortion_cam_par ('fixed', CamParam, [0.0,0.0,0.0,0.0,0.0], CamParamOut) 24 | change_radial_distortion_image (ImageResized, ImageResized, ImageRectified, CamParam, CamParamOut) 25 | 26 | * Display window. If used in HDev engine, you must open window here. 27 | get_image_size (ImageRectified, Width, Height) 28 | dev_open_window (0, 0, Width, Height, 'black', WindowHandle) 29 | dev_display (ImageRectified) 30 | 31 | * Close the framegrabber 32 | close_framegrabber (AcqHandle) 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /image_acquisition/images/cake.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rbrth/framework/878c795311dc94443bcffa584865a2ae177d38bf/image_acquisition/images/cake.jpg -------------------------------------------------------------------------------- /image_acquisition/include/image_acquisition_node.h: -------------------------------------------------------------------------------- 1 | #ifndef IMAGE_ACQUISITION_NODE_H 2 | #define IMAGE_ACQUISITION_NODE_H 3 | 4 | 5 | //openCV FIRST, otherwise it does not compile. Do not ask why.. 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "ros/ros.h" 12 | #include "std_msgs/String.h" 13 | #include "std_msgs/Time.h" 14 | #include "sensor_msgs/Image.h" 15 | #include 16 | #include 17 | #include 18 | 19 | #include "HalconCpp.h" 20 | #include "HDevEngineCpp.h" 21 | #include "window_handling.h" 22 | 23 | #include 24 | #include 25 | 26 | 27 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 28 | The global variables in this frame are used in the HDevEngine. 29 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 30 | HDevEngineCpp::HDevEngine hdevengine; 31 | HDevEngineCpp::HDevProgram hdev_script; 32 | HDevEngineCpp::HDevProgramCall script_results; 33 | 34 | std::string image_acquisition_halcon_script_path; 35 | std::string image_acquisition_halcon_external_procedures_path; 36 | std::string camera_parameters_path; 37 | 38 | std::string initialize_camera_function; 39 | std::string grab_rectified_rgb_image_function; 40 | std::string grab_rectified_gray_image_function; 41 | std::string grab_rectified_colorspace_image_function; 42 | std::string display_grabbed_image; 43 | std::string close_frame_grabber_function; 44 | 45 | HalconCpp::HTuple camera_parameters; 46 | bool camera_parameters_loaded; 47 | 48 | bool global_lock; 49 | bool initialized; 50 | 51 | HalconCpp::HTuple camera_acquisition_handle; 52 | 53 | HalconCpp::HImage most_recent_image_halcon_rgb; 54 | std_msgs::Time most_recent_image_halcon_rgb_timestamp; 55 | bool rgb_image_taken; 56 | 57 | HalconCpp::HImage most_recent_image_halcon_gray; 58 | std_msgs::Time most_recent_image_halcon_gray_timestamp; 59 | bool gray_image_taken; 60 | 61 | HalconCpp::HImage most_recent_image_halcon_colorspace; 62 | std_msgs::Time most_recent_image_halcon_colorspace_timestamp; 63 | bool colorspace_image_taken; 64 | 65 | 66 | 67 | sensor_msgs::Image most_recent_image_ros_rgb; 68 | std_msgs::Time most_recent_image_ros_rgb_timestamp; 69 | 70 | int exposure; 71 | int gain; 72 | int gainR; 73 | int gainG; 74 | int gainB; 75 | 76 | int hue_lower_threshold; 77 | int hue_upper_threshold; 78 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 87 | Global publishers and Subscribers of the ROS node 88 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 89 | ros::Publisher rgb_image_publisher; 90 | ros::Publisher gray_image_publisher; 91 | ros::Publisher colorspace_image_publisher; 92 | ros::ServiceServer camera_parameters_service; 93 | ros::ServiceServer image_service_rgb; 94 | ros::ServiceServer image_service_gray; 95 | ros::ServiceServer image_service_colorspace; 96 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 97 | 98 | 99 | 100 | 101 | 102 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 103 | All functions within this ros node. 104 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 105 | sensor_msgs::Image convertRGBImage_Halcon2Ros(HalconCpp::HImage halcon_image, std_msgs::Time timestamp); 106 | sensor_msgs::Image convertGRAYImage_Halcon2Ros(HalconCpp::HImage halcon_image, std_msgs::Time timestamp); 107 | sensor_msgs::Image convertCOLORSPACEImage_Halcon2Ros(HalconCpp::HImage halcon_image, std_msgs::Time timestamp); 108 | 109 | void publishImage(HalconCpp::HImage image, std::string type); 110 | 111 | bool externalTrigger_RectifiedRGBImage(image_acquisition::request_image::Request &req, image_acquisition::request_image::Response &res); 112 | bool externalTrigger_RectifiedGRAYImage(image_acquisition::request_image::Request &req, image_acquisition::request_image::Response &res); 113 | bool externalTrigger_RectifiedCOLORSPACEImage(image_acquisition::request_image::Request &req, image_acquisition::request_image::Response &res); 114 | 115 | bool getCameraParameters(image_acquisition::request_camera_parameters::Request &req, image_acquisition::request_camera_parameters::Response &res); 116 | 117 | void grabRectifiedImageRGB(HalconCpp::HImage &halcon_image, std_msgs::Time ×tamp); 118 | void grabRectifiedImageGRAY(HalconCpp::HImage &halcon_image, std_msgs::Time ×tamp); 119 | void grabRectifiedImageCOLORSPACE(HalconCpp::HImage &halcon_image, std_msgs::Time ×tamp); 120 | 121 | 122 | void initializeCamera(); 123 | void grabRectifiedImage(); 124 | void displayGrabbedImage(); 125 | void closeFrameGrabber(); 126 | 127 | void initializeHalconScripts(); 128 | 129 | int main(int argc, char **argv); 130 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 131 | 132 | 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /image_acquisition/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | image_acquisition 4 | 5 | s0.0.0 6 | 7 | This package provides nodes for acquiring images. 8 | 9 | Ruud Barth 10 | 11 | BSD 12 | 13 | catkin 14 | 15 | message_generation 16 | tf 17 | geometry_msgs 18 | roscpp 19 | rospy 20 | std_msgs 21 | visualization_msgs 22 | sensor_msgs 23 | 24 | 25 | 26 | opencv2 27 | cv_bridge 28 | image_transport 29 | opencv2 30 | cv_bridge 31 | image_transport 32 | 33 | 34 | 35 | 36 | message_runtime 37 | roscpp 38 | rospy 39 | std_msgs 40 | visualization_msgs 41 | tf 42 | geometry_msgs 43 | sensor_msgs 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /image_acquisition/srv/request_camera_parameters.srv: -------------------------------------------------------------------------------- 1 | string request_message 2 | --- 3 | float64 f 4 | float64 k1 5 | float64 k2 6 | float64 k3 7 | float64 P1 8 | float64 P2 9 | float64 Sx 10 | float64 Sy 11 | float64 Cx 12 | float64 Cy 13 | float64 Iw 14 | float64 Ih -------------------------------------------------------------------------------- /image_acquisition/srv/request_image.srv: -------------------------------------------------------------------------------- 1 | string request_message 2 | --- 3 | sensor_msgs/Image image 4 | -------------------------------------------------------------------------------- /robot_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(robot_control) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | geometry_msgs 12 | message_generation 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 35 | ## pulled in transitively but can be declared for certainty nonetheless: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a run_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | add_service_files( 56 | FILES 57 | request_endpointstate.srv 58 | ) 59 | 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | 69 | find_package(catkin REQUIRED genmsg actionlib_msgs actionlib) 70 | 71 | add_action_files( 72 | FILES 73 | MoveArm.action 74 | ) 75 | generate_messages(DEPENDENCIES actionlib_msgs geometry_msgs) 76 | 77 | ################################### 78 | ## catkin specific configuration ## 79 | ################################### 80 | ## The catkin_package macro generates cmake config files for your package 81 | ## Declare things to be passed to dependent projects 82 | ## INCLUDE_DIRS: uncomment this if you package contains header files 83 | ## LIBRARIES: libraries you create in this project that dependent projects also need 84 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 85 | ## DEPENDS: system dependencies of this project that dependent projects also need 86 | catkin_package( 87 | # INCLUDE_DIRS include 88 | # LIBRARIES baxter 89 | CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs 90 | # DEPENDS system_lib 91 | ) 92 | 93 | ########### 94 | ## Build ## 95 | ########### 96 | 97 | ## Specify additional locations of header files 98 | ## Your package locations should be listed before other locations 99 | # include_directories(include) 100 | include_directories( 101 | ${catkin_INCLUDE_DIRS} 102 | ) 103 | 104 | ## Declare a cpp library 105 | # add_library(baxter 106 | # src/${PROJECT_NAME}/baxter.cpp 107 | # ) 108 | 109 | ## Declare a cpp executable 110 | # add_executable(baxter_node src/baxter_node.cpp) 111 | 112 | ## Add cmake target dependencies of the executable/library 113 | ## as an example, message headers may need to be generated before nodes 114 | # add_dependencies(baxter_node baxter_generate_messages_cpp) 115 | 116 | ## Specify libraries to link a library or executable target against 117 | # target_link_libraries(baxter_node 118 | # ${catkin_LIBRARIES} 119 | # ) 120 | 121 | ############# 122 | ## Install ## 123 | ############# 124 | 125 | # all install targets should use catkin DESTINATION variables 126 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 127 | 128 | ## Mark executable scripts (Python etc.) for installation 129 | ## in contrast to setup.py, you can choose the destination 130 | # install(PROGRAMS 131 | # scripts/my_python_script 132 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 133 | # ) 134 | 135 | ## Mark executables and/or libraries for installation 136 | # install(TARGETS baxter baxter_node 137 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 138 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 139 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 140 | # ) 141 | 142 | ## Mark cpp header files for installation 143 | # install(DIRECTORY include/${PROJECT_NAME}/ 144 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 145 | # FILES_MATCHING PATTERN "*.h" 146 | # PATTERN ".svn" EXCLUDE 147 | # ) 148 | 149 | ## Mark other files for installation (e.g. launch and bag files, etc.) 150 | # install(FILES 151 | # # myfile1 152 | # # myfile2 153 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 154 | # ) 155 | 156 | ############# 157 | ## Testing ## 158 | ############# 159 | 160 | ## Add gtest based cpp test target and link libraries 161 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_baxter.cpp) 162 | # if(TARGET ${PROJECT_NAME}-test) 163 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 164 | # endif() 165 | 166 | ## Add folders to be run by python nosetests 167 | # catkin_add_nosetests(test) 168 | 169 | 170 | 171 | add_executable(robot_control_action_server 172 | src/robot_control_action_server.cpp 173 | ) 174 | 175 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 176 | 177 | target_link_libraries(robot_control_action_server yaml-cpp ${catkin_LIBRARIES}) 178 | 179 | add_dependencies(robot_control_action_server ${catkin_EXPORTED_TARGETS}) 180 | 181 | -------------------------------------------------------------------------------- /robot_control/action/MoveArm.action: -------------------------------------------------------------------------------- 1 | # Define the goal 2 | geometry_msgs/PoseStamped goal_pose 3 | --- 4 | # Define the result 5 | bool goal_reached 6 | --- 7 | # Define a feedback message 8 | float32 distance_to_goal 9 | -------------------------------------------------------------------------------- /robot_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_control 4 | 0.0.0 5 | The baxter package 6 | 7 | 8 | 9 | 10 | ruud 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | geometry_msgs 47 | roscpp 48 | rospy 49 | std_msgs 50 | geometry_msgs 51 | 52 | 53 | actionlib 54 | actionlib_msgs 55 | actionlib 56 | actionlib_msgs 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /robot_control/srv/request_endpointstate.srv: -------------------------------------------------------------------------------- 1 | string request_message 2 | --- 3 | geometry_msgs/PoseStamped pose 4 | -------------------------------------------------------------------------------- /slam_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(slam_bridge) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | roscpp 10 | rospy 11 | std_msgs 12 | sensor_msgs 13 | message_generation 14 | tf 15 | pcl_conversions 16 | pcl_ros 17 | ) 18 | 19 | 20 | # Sophus is required to use the camToWorld field in the KeyFrame.msg from lsd-slam. 21 | ADD_SUBDIRECTORY(${PROJECT_SOURCE_DIR}/thirdparty/Sophus) 22 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/thirdparty/Sophus) 23 | find_package(Eigen3 REQUIRED) 24 | include_directories( ${EIGEN3_INCLUDE_DIR} ) 25 | include_directories( 26 | ${PROJECT_SOURCE_DIR}/thirdparty/Sophus 27 | ) 28 | 29 | 30 | ## System dependencies are found with CMake's conventions 31 | # find_package(Boost REQUIRED COMPONENTS system) 32 | 33 | 34 | ## Uncomment this if the package has a setup.py. This macro ensures 35 | ## modules and global scripts declared therein get installed 36 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 37 | # catkin_python_setup() 38 | 39 | ################################################ 40 | ## Declare ROS messages, services and actions ## 41 | ################################################ 42 | 43 | ## To declare and build messages, services or actions from within this 44 | ## package, follow these steps: 45 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 46 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 47 | ## * In the file package.xml: 48 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 49 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 50 | ## pulled in transitively but can be declared for certainty nonetheless: 51 | ## * add a build_depend tag for "message_generation" 52 | ## * add a run_depend tag for "message_runtime" 53 | ## * In this file (CMakeLists.txt): 54 | ## * add "message_generation" and every package in MSG_DEP_SET to 55 | ## find_package(catkin REQUIRED COMPONENTS ...) 56 | ## * add "message_runtime" and every package in MSG_DEP_SET to 57 | ## catkin_package(CATKIN_DEPENDS ...) 58 | ## * uncomment the add_*_files sections below as needed 59 | ## and list every .msg/.srv/.action file to be processed 60 | ## * uncomment the generate_messages entry below 61 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 62 | 63 | ## Generate messages in the 'msg' folder 64 | add_message_files( 65 | FILES 66 | keyframeMsg.msg 67 | keyframeGraphMsg.msg 68 | ) 69 | 70 | ## Generate services in the 'srv' folder 71 | # add_service_files( 72 | # FILES 73 | # Service1.srv 74 | # Service2.srv 75 | # ) 76 | 77 | ## Generate actions in the 'action' folder 78 | # add_action_files( 79 | # FILES 80 | # Action1.action 81 | # Action2.action 82 | # ) 83 | 84 | ## Generate added messages and services with any dependencies listed here 85 | generate_messages( 86 | DEPENDENCIES 87 | ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES slam 101 | # CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | include_directories( 113 | ${catkin_INCLUDE_DIRS} 114 | ) 115 | 116 | 117 | include_directories(include) 118 | 119 | 120 | ## Declare a cpp library 121 | # add_library(slam 122 | # src/${PROJECT_NAME}/slam.cpp 123 | # ) 124 | 125 | 126 | find_package(PCL 1.6 REQUIRED) 127 | 128 | include_directories(${PCL_INCLUDE_DIRS}) 129 | link_directories(${PCL_LIBRARY_DIRS}) 130 | add_definitions(${PCL_DEFINITIONS}) 131 | 132 | 133 | ## Declare a cpp executable 134 | add_executable(lsd_slam_pointcloud_publisher src/lsd_slam_pointcloud_publisher.cpp) 135 | 136 | ## Specify libraries to link a library or executable target against 137 | target_link_libraries(lsd_slam_pointcloud_publisher 138 | ${catkin_LIBRARIES} 139 | ) 140 | 141 | target_link_libraries (lsd_slam_pointcloud_publisher ${PCL_LIBRARIES}) 142 | 143 | 144 | 145 | 146 | 147 | ############# 148 | ## Install ## 149 | ############# 150 | 151 | # all install targets should use catkin DESTINATION variables 152 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 153 | 154 | ## Mark executable scripts (Python etc.) for installation 155 | ## in contrast to setup.py, you can choose the destination 156 | # install(PROGRAMS 157 | # scripts/my_python_script 158 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 159 | # ) 160 | 161 | ## Mark executables and/or libraries for installation 162 | # install(TARGETS slam slam_node 163 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 164 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 165 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark cpp header files for installation 169 | # install(DIRECTORY include/${PROJECT_NAME}/ 170 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 171 | # FILES_MATCHING PATTERN "*.h" 172 | # PATTERN ".svn" EXCLUDE 173 | # ) 174 | 175 | ## Mark other files for installation (e.g. launch and bag files, etc.) 176 | # install(FILES 177 | # # myfile1 178 | # # myfile2 179 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 180 | # ) 181 | 182 | ############# 183 | ## Testing ## 184 | ############# 185 | 186 | ## Add gtest based cpp test target and link libraries 187 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_slam.cpp) 188 | # if(TARGET ${PROJECT_NAME}-test) 189 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 190 | # endif() 191 | 192 | ## Add folders to be run by python nosetests 193 | # catkin_add_nosetests(test) 194 | -------------------------------------------------------------------------------- /slam_bridge/calibration_files/pre-rectified_images.cfg: -------------------------------------------------------------------------------- 1 | 0.49 0.65 0.48 0.59 0 2 | 640 480 3 | none 4 | 640 480 -------------------------------------------------------------------------------- /slam_bridge/calibration_files/readme.txt: -------------------------------------------------------------------------------- 1 | 3.1.3 Camera Calibration (source: https://github.com/tum-vision/lsd_slam) 2 | 3 | LSD-SLAM operates on a pinhole camera model, however we give the option to undistort images before 4 | they are being used. You can find some sample calib files in lsd_slam_core/calib. 5 | Calibration File for FOV camera model: 6 | 7 | fx/width fy/height cx/width cy/height d 8 | in_width in_height 9 | "crop" / "full" / "none" / "e1 e2 e3 e4 0" 10 | out_width out_height 11 | 12 | Here, the values in the first line are the camera intrinsics and radial distortion parameter as 13 | given by the PTAM cameracalibrator, in_width and in_height is the input image size, and out_width 14 | out_height is the desired undistorted image size. The latter can be chosen freely, however 640x480 15 | is recommended as explained in section 3.1.6. The third line specifies how the image is distorted, 16 | either by specifying a desired camera matrix in the same format as the first four intrinsic 17 | parameters, or by specifying "crop", which crops the image to maximal size while including 18 | only valid image pixels. 19 | 20 | Calibration File for Pre-Rectified Images: 21 | 22 | This one is without radial distortion correction, as a special case of ATAN camera model but without the computational cost: 23 | 24 | fx/width fy/height cx/width cy/height 0 25 | width height 26 | none 27 | width height 28 | 29 | 30 | "The focal length fx (for example) is actually the product of the physical focal length of the lens 31 | and the size sx of the individual imager elements (this should make sense because sx has units of 32 | pixels per millimeter while F has units of millimeters, which means that fx is in the required units 33 | of pixels). It is important to keep in mind, though, that sx and sy cannot be measured directly 34 | via any camera calibration process, and neither is the physical focal length F directly measurable. 35 | Only the combinations fx = F*sx and fy = F*sy can be derived without actually dismantling the 36 | camera and measuring its components directly." 37 | -------------------------------------------------------------------------------- /slam_bridge/include/lsd_slam_pointcloud_publisher.h: -------------------------------------------------------------------------------- 1 | #ifndef LSD_SLAM_POINTCLOUD_PUBLISHER_H_ 2 | #define LSD_SLAM_POINTCLOUD_PUBLISHER_H_ 3 | 4 | #include "ros/ros.h" 5 | #include "std_msgs/String.h" 6 | #include "slam_bridge/keyframeMsg.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "pcl_ros/point_cloud.h" 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include "robot_control/request_endpointstate.h" 30 | #include "geometry_msgs/PoseStamped.h" 31 | #include 32 | #include 33 | #include "sophus/sim3.hpp" 34 | #include 35 | #include 36 | #include 37 | 38 | class SlamBridge{ 39 | 40 | private: 41 | 42 | struct InputPointDense 43 | { 44 | float idepth; 45 | float idepth_var; 46 | unsigned char color[4]; 47 | }; 48 | 49 | bool camera_parameters_loaded; 50 | int sparsity_factor; 51 | ros::ServiceClient camera_parameters_client; 52 | ros::ServiceClient robot_endpointstate_client; 53 | ros::Publisher output_pointcloud_publisher; 54 | ros::Subscriber raw_pointcloud_subcriber; 55 | ros::Subscriber slam_pose_subcriber; 56 | InputPointDense* originalInput; 57 | std::string frame; 58 | sensor_msgs::PointCloud2 reconstructed_scene_pointcloud2; 59 | bool lsd_slam_frame_transformed; 60 | 61 | Sophus::Sim3f camToWorld; 62 | 63 | double f; 64 | double k1; 65 | double k2; 66 | double k3; 67 | double P1; 68 | double P2; 69 | double Sx; 70 | double Sy; 71 | double Cx; 72 | double Cy; 73 | double Iw; 74 | double Ih; 75 | 76 | double fxi; 77 | double fyi; 78 | double cxi; 79 | double cyi; 80 | 81 | public: 82 | ros::NodeHandle node_handle; 83 | tf::TransformListener transform_listener; 84 | geometry_msgs::PoseStamped robot_endpointstate_pose; 85 | 86 | SlamBridge(); 87 | void pointcloudCallback(const slam_bridge::keyframeMsg::ConstPtr& raw_point_cloud_message); 88 | void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& slam_pose); 89 | bool requestEndPointState(); 90 | bool requestCameraParameters(); 91 | }; 92 | 93 | #endif /* LSD_SLAM_POINTCLOUD_PUBLISHER_H_ */ 94 | -------------------------------------------------------------------------------- /slam_bridge/msg/keyframeGraphMsg.msg: -------------------------------------------------------------------------------- 1 | # data as serialization of sim(3)'s: (int id, float[7] camToWorld) 2 | uint32 numFrames 3 | uint8[] frameData 4 | 5 | 6 | # constraints (int from, int to, float err) 7 | uint32 numConstraints 8 | uint8[] constraintsData 9 | -------------------------------------------------------------------------------- /slam_bridge/msg/keyframeMsg.msg: -------------------------------------------------------------------------------- 1 | int32 id 2 | float64 time 3 | bool isKeyframe 4 | 5 | # camToWorld as serialization of sophus sim(3). 6 | # may change with keyframeGraph-updates. 7 | float32[7] camToWorld 8 | 9 | 10 | # camera parameter (fx fy cx cy), width, height 11 | # will never change, but required for display. 12 | float32 fx 13 | float32 fy 14 | float32 cx 15 | float32 cy 16 | uint32 height 17 | uint32 width 18 | 19 | 20 | # data as InputPointDense (float idepth, float idepth_var, uchar color[4]), width x height 21 | # may be empty, in that case no associated pointcloud is ever shown. 22 | uint8[] pointcloud 23 | -------------------------------------------------------------------------------- /slam_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | slam_bridge 4 | 0.0.0 5 | The slam package 6 | 7 | 8 | 9 | 10 | ruud 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | geometry_msgs 45 | roscpp 46 | rospy 47 | std_msgs 48 | sensor_msgs 49 | tf 50 | pcl_conversions 51 | pcl_ros 52 | pcl 53 | dynamic_reconfigure 54 | 55 | 56 | geometry_msgs 57 | roscpp 58 | rospy 59 | std_msgs 60 | sensor_msgs 61 | tf 62 | pcl_conversions 63 | pcl_ros 64 | pcl 65 | dynamic_reconfigure 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME Sophus) 2 | 3 | PROJECT(${PROJECT_NAME}) 4 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 5 | 6 | SET (CMAKE_VERBOSE_MAKEFILE ON) 7 | 8 | # Release by default 9 | # Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug" 10 | IF( NOT CMAKE_BUILD_TYPE ) 11 | SET( CMAKE_BUILD_TYPE Release ) 12 | ENDIF() 13 | 14 | IF (CMAKE_COMPILER_IS_GNUCXX ) 15 | SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") 16 | SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG ") 17 | 18 | ADD_DEFINITIONS("-Wall -Werror -Wno-unused-variable 19 | -Wno-unused-but-set-variable -Wno-unknown-pragmas ") 20 | ENDIF() 21 | 22 | ################################################################################ 23 | # Add local path for finding packages, set the local version first 24 | set( CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake_modules" ) 25 | list( APPEND CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" ) 26 | 27 | ################################################################################ 28 | # Create variables used for exporting in SophusConfig.cmake 29 | set( Sophus_LIBRARIES "" ) 30 | set( Sophus_INCLUDE_DIR ${PROJECT_SOURCE_DIR} ) 31 | 32 | ################################################################################ 33 | 34 | include(FindEigen3.cmake) 35 | #find_package( Eigen3 REQUIRED ) 36 | INCLUDE_DIRECTORIES( ${EIGEN3_INCLUDE_DIR} ) 37 | SET( Sophus_INCLUDE_DIR ${Sophus_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ) 38 | 39 | SET (SOURCE_DIR "sophus") 40 | 41 | SET (TEMPLATES tests 42 | so2 43 | se2 44 | so3 45 | se3 46 | rxso3 47 | sim3 48 | ) 49 | 50 | SET (SOURCES ${SOURCE_DIR}/sophus.hpp) 51 | 52 | FOREACH(templ ${TEMPLATES}) 53 | LIST(APPEND SOURCES ${SOURCE_DIR}/${templ}.hpp) 54 | ENDFOREACH(templ) 55 | 56 | 57 | INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) 58 | 59 | # Added ${SOURCES} to executables so they show up in QtCreator (and possibly 60 | # other IDEs). 61 | # ADD_EXECUTABLE(test_so2 sophus/test_so2.cpp ${SOURCES}) 62 | # ADD_EXECUTABLE(test_se2 sophus/test_se2.cpp ${SOURCES}) 63 | # ADD_EXECUTABLE(test_so3 sophus/test_so3.cpp ${SOURCES}) 64 | # ADD_EXECUTABLE(test_se3 sophus/test_se3.cpp ${SOURCES}) 65 | # ADD_EXECUTABLE(test_rxso3 sophus/test_rxso3.cpp ${SOURCES}) 66 | # ADD_EXECUTABLE(test_sim3 sophus/test_sim3.cpp ${SOURCES}) 67 | # ENABLE_TESTING() 68 | # 69 | # ADD_TEST(test_so2 test_so2) 70 | # ADD_TEST(test_se2 test_se2) 71 | # ADD_TEST(test_so3 test_so3) 72 | # ADD_TEST(test_se3 test_se3) 73 | # ADD_TEST(test_rxso3 test_rxso3) 74 | # ADD_TEST(test_sim3 test_sim3) 75 | 76 | ################################################################################ 77 | # Create the SophusConfig.cmake file for other cmake projects. 78 | CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/SophusConfig.cmake.in 79 | ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake @ONLY IMMEDIATE ) 80 | export( PACKAGE Sophus ) 81 | 82 | INSTALL(DIRECTORY sophus DESTINATION ${CMAKE_INSTALL_PREFIX}/include 83 | FILES_MATCHING PATTERN "*.hpp" ) -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/README: -------------------------------------------------------------------------------- 1 | Sophus (version 0.9a) 2 | 3 | C++ implementation of Lie Groups using Eigen. 4 | 5 | Thanks to Steven Lovegrove, Sophus is now fully templated - using the Curiously Recurring Template Pattern (CRTP). 6 | 7 | (In order to go back to the non-templated/double-only version "git checkout a621ff".) 8 | 9 | Installation guide: 10 | 11 | >>> 12 | cd Sophus 13 | mkdir build 14 | cd build 15 | cmake .. 16 | make 17 | <<< 18 | 19 | 20 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/SophusConfig.cmake.in: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Sophus source dir 3 | set( Sophus_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") 4 | 5 | ################################################################################ 6 | # Sophus build dir 7 | set( Sophus_DIR "@CMAKE_CURRENT_BINARY_DIR@") 8 | 9 | ################################################################################ 10 | set( Sophus_INCLUDE_DIR "@Sophus_INCLUDE_DIR@" ) 11 | set( Sophus_INCLUDE_DIRS "@Sophus_INCLUDE_DIR@" ) -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # Once done this will define 3 | # 4 | # EIGEN3_FOUND - system has eigen lib 5 | # EIGEN3_INCLUDE_DIR - the eigen include directory 6 | 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Redistribution and use is allowed according to the terms of the BSD license. 9 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 10 | 11 | if( EIGEN3_INCLUDE_DIR ) 12 | # in cache already 13 | set( EIGEN3_FOUND TRUE ) 14 | else (EIGEN3_INCLUDE_DIR) 15 | find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core 16 | PATH_SUFFIXES eigen3/ 17 | HINTS 18 | ${INCLUDE_INSTALL_DIR} 19 | /usr/local/include 20 | ${KDE4_INCLUDE_DIR} 21 | ) 22 | include( FindPackageHandleStandardArgs ) 23 | find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR ) 24 | mark_as_advanced( EIGEN3_INCLUDE_DIR ) 25 | endif(EIGEN3_INCLUDE_DIR) 26 | 27 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/sophus/sophus.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef SOPHUS_HPP 24 | #define SOPHUS_HPP 25 | 26 | #include 27 | 28 | // fix log1p not being found on Android in Eigen 29 | #if defined( ANDROID ) 30 | #include 31 | namespace std { 32 | using ::log1p; 33 | } 34 | #endif 35 | 36 | #include 37 | #include 38 | 39 | namespace Sophus { 40 | using namespace Eigen; 41 | 42 | template 43 | struct SophusConstants { 44 | EIGEN_ALWAYS_INLINE static 45 | const Scalar epsilon() { 46 | return static_cast(1e-10); 47 | } 48 | 49 | EIGEN_ALWAYS_INLINE static 50 | const Scalar pi() { 51 | return static_cast(M_PI); 52 | } 53 | }; 54 | 55 | template<> 56 | struct SophusConstants { 57 | EIGEN_ALWAYS_INLINE static 58 | float epsilon() { 59 | return static_cast(1e-5); 60 | } 61 | 62 | EIGEN_ALWAYS_INLINE static 63 | float pi() { 64 | return static_cast(M_PI); 65 | } 66 | }; 67 | 68 | class SophusException : public std::runtime_error { 69 | public: 70 | SophusException (const std::string& str) 71 | : runtime_error("Sophus exception: " + str) { 72 | } 73 | }; 74 | 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/sophus/test_rxso3.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | 27 | #include "rxso3.hpp" 28 | #include "tests.hpp" 29 | 30 | using namespace Sophus; 31 | using namespace std; 32 | 33 | template 34 | void tests() { 35 | 36 | typedef RxSO3Group RxSO3Type; 37 | typedef typename RxSO3Group::Point Point; 38 | typedef typename RxSO3Group::Tangent Tangent; 39 | 40 | vector rxso3_vec; 41 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0, 1.))); 42 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, -1.0, 1.1))); 43 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0., 1.1))); 44 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.))); 45 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.00001))); 46 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0))); 47 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(M_PI, 0, 0, 0.9))); 48 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0,0)) 49 | *RxSO3Type::exp(Tangent(M_PI, 0, 0,0.0)) 50 | *RxSO3Type::exp(Tangent(-0.2, -0.5, -0.0,0))); 51 | rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.3, 0.5, 0.1,0)) 52 | *RxSO3Type::exp(Tangent(M_PI, 0, 0,0)) 53 | *RxSO3Type::exp(Tangent(-0.3, -0.5, -0.1,0))); 54 | 55 | vector tangent_vec; 56 | Tangent tmp; 57 | tmp << 0,0,0,0; 58 | tangent_vec.push_back(tmp); 59 | tmp << 1,0,0,0; 60 | tangent_vec.push_back(tmp); 61 | tmp << 1,0,0,0.1; 62 | tangent_vec.push_back(tmp); 63 | tmp << 0,1,0,0.1; 64 | tangent_vec.push_back(tmp); 65 | tmp << 0,0,1,-0.1; 66 | tangent_vec.push_back(tmp); 67 | tmp << -1,1,0,-0.1; 68 | tangent_vec.push_back(tmp); 69 | tmp << 20,-1,0,2; 70 | tangent_vec.push_back(tmp); 71 | 72 | vector point_vec; 73 | point_vec.push_back(Point(1,2,4)); 74 | 75 | Tests tests; 76 | tests.setGroupElements(rxso3_vec); 77 | tests.setTangentVectors(tangent_vec); 78 | tests.setPoints(point_vec); 79 | 80 | tests.runAllTests(); 81 | } 82 | 83 | int main() { 84 | cerr << "Test RxSO3" << endl << endl; 85 | 86 | cerr << "Double tests: " << endl; 87 | tests(); 88 | 89 | cerr << "Float tests: " << endl; 90 | tests(); 91 | return 0; 92 | } 93 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/sophus/test_se2.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include "se2.hpp" 28 | #include "tests.hpp" 29 | 30 | using namespace Sophus; 31 | using namespace std; 32 | 33 | template 34 | void tests() { 35 | 36 | typedef SO2Group SO2Type; 37 | typedef SE2Group SE2Type; 38 | typedef typename SE2Group::Point Point; 39 | typedef typename SE2Group::Tangent Tangent; 40 | 41 | vector se2_vec; 42 | se2_vec.push_back(SE2Type(SO2Type(0.0),Point(0,0))); 43 | se2_vec.push_back(SE2Type(SO2Type(0.2),Point(10,0))); 44 | se2_vec.push_back(SE2Type(SO2Type(0.),Point(0,100))); 45 | se2_vec.push_back(SE2Type(SO2Type(-1.),Point(20,-1))); 46 | se2_vec.push_back(SE2Type(SO2Type(0.00001), 47 | Point(-0.00000001,0.0000000001))); 48 | se2_vec.push_back(SE2Type(SO2Type(0.2),Point(0,0)) 49 | *SE2Type(SO2Type(M_PI),Point(0,0)) 50 | *SE2Type(SO2Type(-0.2),Point(0,0))); 51 | se2_vec.push_back(SE2Type(SO2Type(0.3),Point(2,0)) 52 | *SE2Type(SO2Type(M_PI),Point(0,0)) 53 | *SE2Type(SO2Type(-0.3),Point(0,6))); 54 | 55 | vector tangent_vec; 56 | Tangent tmp; 57 | tmp << 0,0,0; 58 | tangent_vec.push_back(tmp); 59 | tmp << 1,0,0; 60 | tangent_vec.push_back(tmp); 61 | tmp << 0,1,1; 62 | tangent_vec.push_back(tmp); 63 | tmp << -1,1,0; 64 | tangent_vec.push_back(tmp); 65 | tmp << 20,-1,-1; 66 | tangent_vec.push_back(tmp); 67 | tmp << 30,5,20; 68 | tangent_vec.push_back(tmp); 69 | 70 | vector point_vec; 71 | point_vec.push_back(Point(1,2)); 72 | 73 | Tests tests; 74 | tests.setGroupElements(se2_vec); 75 | tests.setTangentVectors(tangent_vec); 76 | tests.setPoints(point_vec); 77 | 78 | tests.runAllTests(); 79 | } 80 | 81 | int main() { 82 | cerr << "Test SE2" << endl << endl; 83 | 84 | cerr << "Double tests: " << endl; 85 | tests(); 86 | 87 | cerr << "Float tests: " << endl; 88 | tests(); 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/sophus/test_se3.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include "se3.hpp" 27 | #include "tests.hpp" 28 | 29 | using namespace Sophus; 30 | using namespace std; 31 | 32 | template 33 | void tests() { 34 | 35 | typedef SO3Group SO3Type; 36 | typedef SE3Group SE3Type; 37 | typedef typename SE3Group::Point Point; 38 | typedef typename SE3Group::Tangent Tangent; 39 | 40 | vector se3_vec; 41 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), 42 | Point(0,0,0))); 43 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), 44 | Point(10,0,0))); 45 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)), 46 | Point(0,100,5))); 47 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), 48 | Point(0,0,0))); 49 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), 50 | Point(0,-0.00000001,0.0000000001))); 51 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), 52 | Point(0.01,0,0))); 53 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), 54 | Point(4,-5,0))); 55 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), 56 | Point(0,0,0)) 57 | *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), 58 | Point(0,0,0)) 59 | *SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), 60 | Point(0,0,0))); 61 | se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), 62 | Point(2,0,-7)) 63 | *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), 64 | Point(0,0,0)) 65 | *SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), 66 | Point(0,6,0))); 67 | vector tangent_vec; 68 | Tangent tmp; 69 | tmp << 0,0,0,0,0,0; 70 | tangent_vec.push_back(tmp); 71 | tmp << 1,0,0,0,0,0; 72 | tangent_vec.push_back(tmp); 73 | tmp << 0,1,0,1,0,0; 74 | tangent_vec.push_back(tmp); 75 | tmp << 0,-5,10,0,0,0; 76 | tangent_vec.push_back(tmp); 77 | tmp << -1,1,0,0,0,1; 78 | tangent_vec.push_back(tmp); 79 | tmp << 20,-1,0,-1,1,0; 80 | tangent_vec.push_back(tmp); 81 | tmp << 30,5,-1,20,-1,0; 82 | tangent_vec.push_back(tmp); 83 | 84 | vector point_vec; 85 | point_vec.push_back(Point(1,2,4)); 86 | 87 | Tests tests; 88 | tests.setGroupElements(se3_vec); 89 | tests.setTangentVectors(tangent_vec); 90 | tests.setPoints(point_vec); 91 | 92 | tests.runAllTests(); 93 | cerr << "passed." << endl << endl; 94 | } 95 | 96 | int main() { 97 | cerr << "Test SE3" << endl << endl; 98 | 99 | cerr << "Double tests: " << endl; 100 | tests(); 101 | 102 | cerr << "Float tests: " << endl; 103 | tests(); 104 | return 0; 105 | } 106 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/sophus/test_sim3.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | #include "sim3.hpp" 29 | #include "tests.hpp" 30 | 31 | using namespace Sophus; 32 | using namespace std; 33 | 34 | template 35 | void tests() { 36 | 37 | typedef Sim3Group Sim3Type; 38 | typedef RxSO3Group RxSO3Type; 39 | typedef typename Sim3Group::Point Point; 40 | typedef typename Sim3Group::Tangent Tangent; 41 | typedef Matrix Vector4Type; 42 | 43 | vector sim3_vec; 44 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,1.)), 45 | Point(0,0,0))); 46 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, -1.0,1.1)), 47 | Point(10,0,0))); 48 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.,1.1)), 49 | Point(0,10,5))); 50 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0.)), 51 | Point(0,0,0))); 52 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp( 53 | Vector4Type(0., 0., 0.00001, 0.0000001)), 54 | Point(1,-1.00000001,2.0000000001))); 55 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0)), 56 | Point(0.01,0,0))); 57 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0.9)), 58 | Point(4,-5,0))); 59 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,0)), 60 | Point(0,0,0)) 61 | *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)), 62 | Point(0,0,0)) 63 | *Sim3Type(RxSO3Type::exp(Vector4Type(-0.2, -0.5, -0.0,0)), 64 | Point(0,0,0))); 65 | sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.3, 0.5, 0.1,0)), 66 | Point(2,0,-7)) 67 | *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)), 68 | Point(0,0,0)) 69 | *Sim3Type(RxSO3Type::exp(Vector4Type(-0.3, -0.5, -0.1,0)), 70 | Point(0,6,0))); 71 | vector tangent_vec; 72 | Tangent tmp; 73 | tmp << 0,0,0,0,0,0,0; 74 | tangent_vec.push_back(tmp); 75 | tmp << 1,0,0,0,0,0,0; 76 | tangent_vec.push_back(tmp); 77 | tmp << 0,1,0,1,0,0,0.1; 78 | tangent_vec.push_back(tmp); 79 | tmp << 0,0,1,0,1,0,0.1; 80 | tangent_vec.push_back(tmp); 81 | tmp << -1,1,0,0,0,1,-0.1; 82 | tangent_vec.push_back(tmp); 83 | tmp << 20,-1,0,-1,1,0,-0.1; 84 | tangent_vec.push_back(tmp); 85 | tmp << 30,5,-1,20,-1,0,1.5; 86 | tangent_vec.push_back(tmp); 87 | 88 | 89 | vector point_vec; 90 | point_vec.push_back(Point(1,2,4)); 91 | 92 | Tests tests; 93 | tests.setGroupElements(sim3_vec); 94 | tests.setTangentVectors(tangent_vec); 95 | tests.setPoints(point_vec); 96 | 97 | tests.runAllTests(); 98 | } 99 | 100 | int main() { 101 | cerr << "Test Sim3" << endl << endl; 102 | 103 | cerr << "Double tests: " << endl; 104 | tests(); 105 | 106 | cerr << "Float tests: " << endl; 107 | tests(); 108 | return 0; 109 | } 110 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/sophus/test_so2.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | 24 | #include 25 | #include 26 | 27 | #include "so2.hpp" 28 | #include "tests.hpp" 29 | 30 | using namespace Sophus; 31 | using namespace std; 32 | 33 | template 34 | void tests() { 35 | 36 | typedef SO2Group SO2Type; 37 | typedef typename SO2Group::Point Point; 38 | typedef typename SO2Group::Tangent Tangent; 39 | 40 | vector so2_vec; 41 | so2_vec.push_back(SO2Type::exp(0.0)); 42 | so2_vec.push_back(SO2Type::exp(0.2)); 43 | so2_vec.push_back(SO2Type::exp(10.)); 44 | so2_vec.push_back(SO2Type::exp(0.00001)); 45 | so2_vec.push_back(SO2Type::exp(M_PI)); 46 | so2_vec.push_back(SO2Type::exp(0.2) 47 | *SO2Type::exp(M_PI) 48 | *SO2Type::exp(-0.2)); 49 | so2_vec.push_back(SO2Type::exp(-0.3) 50 | *SO2Type::exp(M_PI) 51 | *SO2Type::exp(0.3)); 52 | 53 | vector tangent_vec; 54 | tangent_vec.push_back(Tangent(0)); 55 | tangent_vec.push_back(Tangent(1)); 56 | tangent_vec.push_back(Tangent(M_PI_2)); 57 | tangent_vec.push_back(Tangent(-1)); 58 | tangent_vec.push_back(Tangent(20)); 59 | tangent_vec.push_back(Tangent(M_PI_2+0.0001)); 60 | 61 | vector point_vec; 62 | point_vec.push_back(Point(1,2)); 63 | 64 | Tests tests; 65 | tests.setGroupElements(so2_vec); 66 | tests.setTangentVectors(tangent_vec); 67 | tests.setPoints(point_vec); 68 | 69 | tests.runAllTests(); 70 | 71 | cerr << "Exception test: "; 72 | try { 73 | SO2Type so2(0., 0.); 74 | } catch(SophusException & e) { 75 | cerr << "passed." << endl << endl; 76 | return; 77 | } 78 | cerr << "failed!" << endl << endl; 79 | exit(-1); 80 | } 81 | 82 | int main() { 83 | cerr << "Test SO2" << endl << endl; 84 | 85 | cerr << "Double tests: " << endl; 86 | tests(); 87 | 88 | cerr << "Float tests: " << endl; 89 | tests(); 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/sophus/test_so3.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of Sophus. 2 | // 3 | // Copyright 2012-2013 Hauke Strasdat 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #include 24 | #include 25 | 26 | #include "so3.hpp" 27 | #include "tests.hpp" 28 | 29 | using namespace Sophus; 30 | using namespace std; 31 | 32 | template 33 | void tests() { 34 | 35 | typedef SO3Group SO3Type; 36 | typedef typename SO3Group::Point Point; 37 | typedef typename SO3Group::Tangent Tangent; 38 | 39 | vector so3_vec; 40 | 41 | so3_vec.push_back(SO3Type(Quaternion(0.1e-11, 0., 1., 0.))); 42 | so3_vec.push_back(SO3Type(Quaternion(-1,0.00001,0.0,0.0))); 43 | so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0))); 44 | so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, -1.0))); 45 | so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.))); 46 | so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.00001))); 47 | so3_vec.push_back(SO3Type::exp(Point(M_PI, 0, 0))); 48 | so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0)) 49 | *SO3Type::exp(Point(M_PI, 0, 0)) 50 | *SO3Type::exp(Point(-0.2, -0.5, -0.0))); 51 | so3_vec.push_back(SO3Type::exp(Point(0.3, 0.5, 0.1)) 52 | *SO3Type::exp(Point(M_PI, 0, 0)) 53 | *SO3Type::exp(Point(-0.3, -0.5, -0.1))); 54 | 55 | vector tangent_vec; 56 | tangent_vec.push_back(Tangent(0,0,0)); 57 | tangent_vec.push_back(Tangent(1,0,0)); 58 | tangent_vec.push_back(Tangent(0,1,0)); 59 | tangent_vec.push_back(Tangent(M_PI_2,M_PI_2,0.0)); 60 | tangent_vec.push_back(Tangent(-1,1,0)); 61 | tangent_vec.push_back(Tangent(20,-1,0)); 62 | tangent_vec.push_back(Tangent(30,5,-1)); 63 | 64 | vector point_vec; 65 | point_vec.push_back(Point(1,2,4)); 66 | 67 | Tests tests; 68 | tests.setGroupElements(so3_vec); 69 | tests.setTangentVectors(tangent_vec); 70 | tests.setPoints(point_vec); 71 | 72 | tests.runAllTests(); 73 | } 74 | 75 | int main() { 76 | cerr << "Test SO3" << endl << endl; 77 | 78 | cerr << "Double tests: " << endl; 79 | tests(); 80 | 81 | cerr << "Float tests: " << endl; 82 | tests(); 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /slam_bridge/thirdparty/Sophus/sophus/tests.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPUHS_TESTS_HPP 2 | #define SOPUHS_TESTS_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "sophus.hpp" 8 | 9 | namespace Sophus { 10 | 11 | using namespace std; 12 | using namespace Eigen; 13 | 14 | template 15 | class Tests { 16 | 17 | public: 18 | typedef typename LieGroup::Scalar Scalar; 19 | typedef typename LieGroup::Transformation Transformation; 20 | typedef typename LieGroup::Tangent Tangent; 21 | typedef typename LieGroup::Point Point; 22 | typedef typename LieGroup::Adjoint Adjoint; 23 | static const int N = LieGroup::N; 24 | static const int DoF = LieGroup::DoF; 25 | 26 | const Scalar SMALL_EPS; 27 | 28 | Tests() : SMALL_EPS(SophusConstants::epsilon()) { 29 | } 30 | 31 | void setGroupElements(const vector & group_vec) { 32 | group_vec_ = group_vec; 33 | } 34 | 35 | void setTangentVectors(const vector & tangent_vec) { 36 | tangent_vec_ = tangent_vec; 37 | } 38 | 39 | void setPoints(const vector & point_vec) { 40 | point_vec_ = point_vec; 41 | } 42 | 43 | bool adjointTest() { 44 | bool passed = true; 45 | for (size_t i=0; i20.*SMALL_EPS) { 59 | cerr << "Adjoint" << endl; 60 | cerr << "Test case: " << i << "," << j <SMALL_EPS) { 80 | cerr << "G - exp(log(G))" << endl; 81 | cerr << "Test case: " << i << endl; 82 | cerr << DiffT <10.*SMALL_EPS) { 101 | cerr << "expmap(hat(x)) - exp(x)" << endl; 102 | cerr << "Test case: " << i << endl; 103 | cerr << exp_x <SMALL_EPS) { 124 | cerr << "Transform vector" << endl; 125 | cerr << "Test case: " << i << endl; 126 | cerr << (res1-res2) <SMALL_EPS) { 147 | cerr << "Lie Bracket Test" << endl; 148 | cerr << "Test case: " << i << ", " < fastmul_res(fastmul_res_raw); 165 | Eigen::Map group_j_constmap(group_vec_[j].data()); 166 | fastmul_res = group_vec_[i]; 167 | fastmul_res.fastMultiply(group_j_constmap); 168 | Transformation diff = mul_resmat-fastmul_res.matrix(); 169 | Scalar nrm = diff.norm(); 170 | if (isnan(nrm) || nrm>SMALL_EPS) { 171 | cerr << "Map & Multiply" << endl; 172 | cerr << "Test case: " << i << "," << j << endl; 173 | cerr << diff <SMALL_EPS) { 188 | cerr << "Hat-vee Test" << endl; 189 | cerr << "Test case: " << i << endl; 190 | cerr << resDiff << endl; 191 | cerr << endl; 192 | passed = false; 193 | } 194 | } 195 | return passed; 196 | } 197 | 198 | 199 | 200 | void runAllTests() { 201 | bool passed = adjointTest(); 202 | if (!passed) { 203 | cerr << "failed!" << endl << endl; 204 | exit(-1); 205 | } 206 | passed = expLogTest(); 207 | if (!passed) { 208 | cerr << "failed!" << endl << endl; 209 | exit(-1); 210 | } 211 | passed = expMapTest(); 212 | if (!passed) { 213 | cerr << "failed!" << endl << endl; 214 | exit(-1); 215 | } 216 | passed = groupActionTest(); 217 | if (!passed) { 218 | cerr << "failed!" << endl << endl; 219 | exit(-1); 220 | } 221 | passed = lieBracketTest(); 222 | if (!passed) { 223 | cerr << "failed!" << endl << endl; 224 | exit(-1); 225 | } 226 | passed = mapAndMultTest(); 227 | if (!passed) { 228 | cerr << "failed!" << endl << endl; 229 | exit(-1); 230 | } 231 | passed = veeHatTest(); 232 | if (!passed) { 233 | cerr << "failed!" << endl << endl; 234 | exit(-1); 235 | } 236 | cerr << "passed." << endl << endl; 237 | } 238 | 239 | private: 240 | Matrix map(const Matrix & T, 241 | const Matrix & p) { 242 | return T.template topLeftCorner()*p 243 | + T.template topRightCorner(); 244 | } 245 | 246 | Matrix map(const Matrix & T, 247 | const Matrix & p) { 248 | return T*p; 249 | } 250 | 251 | Scalar norm(const Scalar & v) { 252 | return std::abs(v); 253 | } 254 | 255 | Scalar norm(const Matrix & T) { 256 | return T.norm(); 257 | } 258 | 259 | std::vector group_vec_; 260 | std::vector tangent_vec_; 261 | std::vector point_vec_; 262 | }; 263 | } 264 | #endif // TESTS_HPP 265 | -------------------------------------------------------------------------------- /visual_servo_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(visual_servo_control) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | tf 9 | roscpp 10 | rospy 11 | std_msgs 12 | message_generation 13 | geometry_msgs 14 | #VISP 15 | ) 16 | 17 | 18 | find_package(VISP REQUIRED) 19 | if(VISP_FOUND) 20 | include(${VISP_USE_FILE}) 21 | endif(VISP_FOUND) 22 | 23 | 24 | #find_package(Boost REQUIRED COMPONENTS thread) 25 | #include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 26 | 27 | ## System dependencies are found with CMake's conventions 28 | # find_package(Boost REQUIRED COMPONENTS system) 29 | 30 | ## Uncomment this if the package has a setup.py. This macro ensures 31 | ## modules and global scripts declared therein get installed 32 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 33 | # catkin_python_setup() 34 | 35 | ################################################ 36 | ## Declare ROS messages, services and actions ## 37 | ################################################ 38 | 39 | ## To declare and build messages, services or actions from within this 40 | ## package, follow these steps: 41 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 42 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 43 | ## * In the file package.xml: 44 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 45 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 46 | ## pulled in transitively but can be declared for certainty nonetheless: 47 | ## * add a build_depend tag for "message_generation" 48 | ## * add a run_depend tag for "message_runtime" 49 | ## * In this file (CMakeLists.txt): 50 | ## * add "message_generation" and every package in MSG_DEP_SET to 51 | ## find_package(catkin REQUIRED COMPONENTS ...) 52 | ## * add "message_runtime" and every package in MSG_DEP_SET to 53 | ## catkin_package(CATKIN_DEPENDS ...) 54 | ## * uncomment the add_*_files sections below as needed 55 | ## and list every .msg/.srv/.action file to be processed 56 | ## * uncomment the generate_messages entry below 57 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 58 | 59 | ## Generate messages in the 'msg' folder 60 | add_message_files( 61 | FILES 62 | ) 63 | 64 | ## Generate services in the 'srv' folder 65 | add_service_files( 66 | FILES 67 | request_servo_velocity_vector.srv 68 | ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | generate_messages( 79 | DEPENDENCIES 80 | std_msgs 81 | ) 82 | 83 | ################################### 84 | ## catkin specific configuration ## 85 | ################################### 86 | ## The catkin_package macro generates cmake config files for your package 87 | ## Declare things to be passed to dependent projects 88 | ## INCLUDE_DIRS: uncomment this if you package contains header files 89 | ## LIBRARIES: libraries you create in this project that dependent projects also need 90 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 91 | ## DEPENDS: system dependencies of this project that dependent projects also need 92 | catkin_package( 93 | # INCLUDE_DIRS include 94 | # LIBRARIES hdev_engine 95 | CATKIN_DEPENDS roscpp rospy std_msgs 96 | # DEPENDS system_lib 97 | ) 98 | 99 | ########### 100 | ## Build ## 101 | ########### 102 | 103 | ## Specify additional locations of header files 104 | ## Your package locations should be listed before other locations 105 | include_directories(${catkin_INCLUDE_DIRS}) 106 | include_directories(include) 107 | 108 | 109 | ## Declare a cpp library 110 | # add_library(hdev_engine 111 | # src/${PROJECT_NAME}/hdev_engine.cpp 112 | # ) 113 | 114 | ## Declare a cpp executable 115 | add_executable(visual_servo_control_node src/visual_servo_control_node.cpp) 116 | 117 | ## Add cmake target dependencies of the executable/library 118 | ## as an example, message headers may need to be generated before nodes 119 | add_dependencies(visual_servo_control_node visual_servo_control_node_generate_messages_cpp) 120 | 121 | 122 | 123 | ## Specify libraries to link a library or executable target against 124 | target_link_libraries(visual_servo_control_node ${catkin_LIBRARIES}) 125 | #target_link_libraries(visual_servo_control_node ${VISP_LIBRARIES}) 126 | 127 | #target_link_libraries(visual_servo_control_node ${Boost_LIBRARIES} ${catkin_LIBRARIES}) 128 | 129 | 130 | add_executable(cycle_node src/cycle_node.cpp) 131 | target_link_libraries(cycle_node ${catkin_LIBRARIES}) 132 | 133 | -------------------------------------------------------------------------------- /visual_servo_control/include/visual_servo_control_node.h: -------------------------------------------------------------------------------- 1 | #ifndef visual_servo_control_node_H 2 | #define visual_servo_control_node_H 3 | 4 | #include "ros/ros.h" 5 | #include 6 | #include "std_msgs/String.h" 7 | #include "std_msgs/Time.h" 8 | #include "sensor_msgs/Image.h" 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | 39 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 40 | Camera parameters 41 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 42 | bool camera_parameters_loaded; 43 | 44 | double f; 45 | double k1; 46 | double k2; 47 | double k3; 48 | double P1; 49 | double P2; 50 | double Sx; 51 | double Sy; 52 | double Cx; 53 | double Cy; 54 | double Iw; 55 | double Ih; 56 | 57 | double fxi; 58 | double fyi; 59 | double cxi; 60 | double cyi; 61 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 62 | 63 | 64 | 65 | 66 | 67 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 68 | The global variables for the servo simulation 69 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 70 | bool initialized; 71 | bool simulation_initialized; 72 | bool servo_initialized; 73 | bool program_lock; 74 | 75 | // Here we define the desired and initial/current position of the camera as two homogeneous matrices 76 | // Initial is not allowed to be all zero. 77 | 78 | // Desired pose camera relative to object. In other words, target pose of camera relative to object. 79 | vpHomogeneousMatrix cdMo(0, 0, 0.01, 0, 0, 0); 80 | 81 | // Actual pose camera relative object. In other words, position of object in camera frame is known. 82 | vpHomogeneousMatrix cMo(0.15, -0.1, 1,vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50)); 83 | 84 | // For the simulation we need to create two homogeneous transformations wMc and wMo, 85 | // respectively to define the position of the camera, and the position of the object in 86 | // the world frame. 87 | 88 | // Position of camera in world frame (get from robot). 89 | vpHomogeneousMatrix wMc; 90 | 91 | // Position of the object in world frame. 92 | vpHomogeneousMatrix wMo; 93 | 94 | vpPoint point[4]; 95 | vpServo task; 96 | vpFeaturePoint p[4], pd[4]; 97 | vpFeaturePoint p_cog, pd_cog; 98 | int iteration; 99 | 100 | // We create an instance of a free flying camera. 101 | vpSimulatorCamera robot; 102 | 103 | // Current position and orientation of simulated camera. 104 | tf::Vector3 simulated_camera_position; 105 | tf::Quaternion simulated_camera_rotation; 106 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 107 | 108 | 109 | 110 | 111 | 112 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 113 | Global publishers and Subscribers of the ROS node 114 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 115 | ros::ServiceServer velocity_vector_output_service; 116 | ros::ServiceClient camera_parameters_client; 117 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 118 | 119 | 120 | 121 | 122 | /*°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°° 123 | All functions within this ros node. 124 | °°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°°*/ 125 | bool getVelocityVector_CalTab(visual_servo_control::request_servo_velocity_vector::Request &req, visual_servo_control::request_servo_velocity_vector::Response &res); 126 | bool simulateVelocityVectorCalculation(visual_servo_control::request_servo_velocity_vector::Request &req, visual_servo_control::request_servo_velocity_vector::Response &res); 127 | int main(int argc, char **argv); 128 | /*‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡‡*/ 129 | #endif 130 | -------------------------------------------------------------------------------- /visual_servo_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | visual_servo_control 4 | 5 | s0.0.0 6 | 7 | This package provides nodes for visual servoing to a sweet pepper 8 | 9 | Ruud Barth 10 | 11 | BSD 12 | 13 | catkin 14 | 15 | message_generation 16 | tf 17 | geometry_msgs 18 | roscpp 19 | rospy 20 | std_msgs 21 | visualization_msgs 22 | 23 | 24 | message_runtime 25 | roscpp 26 | rospy 27 | std_msgs 28 | visualization_msgs 29 | tf 30 | geometry_msgs 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /visual_servo_control/readme: -------------------------------------------------------------------------------- 1 | ---------------------------------------------------------------------------------------------------- 2 | Installation: 3 | 4 | This package requires the package ros-indigo-visp to be installed. You can easily install this from 5 | the command line with: 6 | 7 | $ sudo apt-get install ros-indigo-visp 8 | 9 | If this node does not compile thereafter and the following error occurs, 10 | 11 | make[2]: *** [ 0%] No rule to make target `/usr/lib/libboost_thread-mt.so', needed by 12 | `/home/ruud/Dropbox/ros/catkin_workspace/devel/lib/visual_servo_control/node'. Stop 13 | 14 | then you can solve this by installing ViSP from source, compiled without OGRE which has this 15 | boost dependency. To do this, please see: 16 | 17 | http://www.irisa.fr/lagadic/visp/documentation/visp-2.10.0/tutorial-install-ubuntu.html 18 | 19 | When you configure CMake, you have to make sure that the CMAKE_INSTALL_PREFIX is set to /usr instead 20 | of the default /usr/local. 21 | 22 | More information on the issue can be found here: 23 | 24 | https://gforge.inria.fr/forum/forum.php?thread_id=33391&forum_id=1295&group_id=397 25 | 26 | ---------------------------------------------------------------------------------------------------- 27 | In order to visualize a simulation of visual servo control: 28 | 29 | $ roslaunch pepper_servo_package pepper_servo_package.launch 30 | $ rosrun pepper_servo_package cycle_node 31 | 32 | Or manually: 33 | 34 | $ roslaunch pepper_servo_package pepper_servo_package.launch 35 | $ rosservice call /visual_servo_control_node/request_servo_velocity_vector '{ request_message: "initialize" }' 36 | $ rosservice call /visual_servo_control_node/request_servo_velocity_vector '{ request_message: "cycle" }' 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /visual_servo_control/src/cycle_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014,DLO, Wageningen University & Research Center. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Open Source Robotics Foundation nor 18 | * the names of its contributors may be used to endorse or promote 19 | * products derived from this software without specific prior 20 | * written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include "ros/ros.h" 37 | #include 38 | #include 39 | 40 | int main(int argc, char **argv) 41 | { 42 | ros::init(argc, argv, "cycle_node"); 43 | ros::NodeHandle n; 44 | ros::ServiceClient client = n.serviceClient("/visual_servo_control_node/request_servo_velocity_vector"); 45 | 46 | bool first = true; 47 | ros::Rate rate(10.0); 48 | while (n.ok()) 49 | { 50 | std::string request_message; 51 | if(first) 52 | { 53 | request_message = "initialize"; 54 | visual_servo_control::request_servo_velocity_vector srv; 55 | srv.request.request_message = request_message; 56 | client.call(srv); 57 | first = false; 58 | } 59 | else 60 | { 61 | request_message = "cycle"; 62 | visual_servo_control::request_servo_velocity_vector srv; 63 | srv.request.request_message = request_message; 64 | 65 | if (client.call(srv)) 66 | { 67 | std::vector velocity_vector(srv.response.servo_velocity_vector ); 68 | ROS_INFO("Velocity vector has been recieved: %f %f %f %f %f %f", velocity_vector[0] , velocity_vector[1] , velocity_vector[2] , velocity_vector[3] , velocity_vector[4] , velocity_vector[5] ); 69 | } 70 | else 71 | { 72 | ROS_INFO("Could not call service."); 73 | } 74 | } 75 | ros::spinOnce(); 76 | rate.sleep(); 77 | } 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /visual_servo_control/srv/request_servo_velocity_vector.srv: -------------------------------------------------------------------------------- 1 | string request_message 2 | float64 pepper_blob_center_of_gravity_x 3 | float64 pepper_blob_center_of_gravity_y 4 | --- 5 | bool initialized 6 | bool found_vector 7 | float64[] servo_velocity_vector 8 | float64 error_x 9 | float64 error_y 10 | --------------------------------------------------------------------------------