├── rviz.png ├── dobot1.jpg ├── dobot2.jpg ├── rviz2.png ├── findobject.png ├── findobject2.png ├── maincontrol.png ├── msg ├── ObjectMsg.msg └── object_position.msg ├── find object └── bin │ ├── session 1.bin │ ├── session 2.bin │ └── session 3.bin ├── LICENSE ├── README.md ├── package.xml ├── src ├── tf_listener.cpp ├── service_call.cpp └── main_control.cpp ├── launch └── CR5_with_realsense.launch └── CMakeLists.txt /rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/rviz.png -------------------------------------------------------------------------------- /dobot1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/dobot1.jpg -------------------------------------------------------------------------------- /dobot2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/dobot2.jpg -------------------------------------------------------------------------------- /rviz2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/rviz2.png -------------------------------------------------------------------------------- /findobject.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/findobject.png -------------------------------------------------------------------------------- /findobject2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/findobject2.png -------------------------------------------------------------------------------- /maincontrol.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/maincontrol.png -------------------------------------------------------------------------------- /msg/ObjectMsg.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float64 z 4 | float64 rx 5 | float64 ry 6 | float64 rz -------------------------------------------------------------------------------- /msg/object_position.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float64 z 4 | float64 rx 5 | float64 ry 6 | float64 rz -------------------------------------------------------------------------------- /find object/bin/session 1.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/find object/bin/session 1.bin -------------------------------------------------------------------------------- /find object/bin/session 2.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/find object/bin/session 2.bin -------------------------------------------------------------------------------- /find object/bin/session 3.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/monkeyrom/CR5_Project/HEAD/find object/bin/session 3.bin -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, Thanakrit Taweesoontorn 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | #
Dobot CR5 with Intel Realsense for Bin-Picking
2 | 3 | Dobot CR5 with Intel Realsense D435i for object detection and pose estimation (bin-picking application). 4 | 5 | This project is present the robot arm application especially bin-picking base on Dobot CR5 using ROS which detect an object and 6d pose estimation with intel realsense d435i depth camera by detection [find-object](https://introlab.github.io/find-object/) app. 6 | 7 | ## Requirement 8 | 9 | - ubuntu 20.04 10 | - ROS noetic 11 | 12 | # Building 13 | 14 | ### Use git to clone the source code 15 | ```sh 16 | cd $HOME/catkin_ws/src 17 | git clone https://github.com/Dobot-Arm/CR_ROS.git 18 | git clone https://github.com/IntelRealSense/realsense-ros.git 19 | git clone https://github.com/introlab/find-object.git 20 | git clone https://github.com/monkeyrom/CR5_Project.git 21 | cd $HOME/catkin_ws 22 | ``` 23 | 24 | ### building 25 | ```sh 26 | catkin_make 27 | ``` 28 | ### set the dobot type 29 | ```sh 30 | echo "export DOBOT_TYPE=cr5" >> ~/.bashrc 31 | source ~/.bashrc 32 | source $HOME/catkin_ws/devel/setup.bash 33 | ``` 34 | 35 | ## 1. Launch Project 36 | 37 | * Connect the robotic arm with following command, and default robot_ip is 192.168.1.6 38 | 39 | ```sh 40 | roslaunch CR5_Project CR5_with_realsense.launch 41 | ``` 42 | 43 | * this command will launch 44 | - dobot_bringup 45 | - realsense camera pointcloud 46 | - find object 2d 47 | - tf synchronisation 48 | 49 | ### rviz display 50 | 51 | ![rviz display](./rviz.png) 52 | 53 | ### find object GUI 54 | 55 | ![find-object](./findobject.png) 56 | 57 | ## 2. Add object image for detection 58 | 59 | * Using find object gui for adding image 60 | - > edit 61 | - > add object from scene 62 | - > take picture 63 | - > crop object 64 | 65 | ![find-object with object](./findobject2.png) 66 | 67 | ### tf synchronize 68 | 69 | ![rviz display tf](./rviz2.png) 70 | 71 | ## 3. Run a terminal for controlling robot 72 | 73 | ```sh 74 | rosrun CR5_Project service_call.launch 75 | ``` 76 | 77 | * this command will run 2 nodes and spawn new terminal for commanding 78 | - service_call 79 | - main_control 80 | 81 | ### new terminal for input command 82 | ![new terminal](./maincontrol.png) 83 | 84 | ## Real Robotic Arm 85 | 86 | ### Dobot CR5 87 | ![Dobot CR5](./dobot1.jpg) 88 | 89 | ### Intel Realsense D435i 90 | ![Intel Realsense D435i](./dobot2.jpg) 91 | 92 | # References 93 | - **CR_ROS**: https://github.com/Dobot-Arm/CR_ROS 94 | - **find-object**: https://github.com/introlab/find-object 95 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | CR5_Project 4 | 0.1.0 5 | The CR5_Project package 6 | 7 | 8 | 9 | 10 | rai 11 | 12 | 13 | 14 | 15 | 16 | ROM 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | message_generation 41 | 42 | 43 | 44 | 45 | 46 | message_runtime 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | 59 | roscpp 60 | rospy 61 | std_msgs 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /src/tf_listener.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "tf/transform_listener.h" 3 | #include "ros/ros.h" 4 | 5 | #define _USE_MATH_DEFINES 6 | class echoListener 7 | { 8 | public: 9 | 10 | tf::TransformListener tf; 11 | 12 | //constructor with name 13 | echoListener() 14 | { 15 | 16 | }; 17 | 18 | ~echoListener() 19 | { 20 | 21 | }; 22 | 23 | private: 24 | 25 | }; 26 | 27 | 28 | int main(int argc, char ** argv) 29 | { 30 | //Initialize ROS 31 | ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName); 32 | 33 | // Allow 2 or 3 command line arguments 34 | if (argc < 3 || argc > 4) 35 | { 36 | printf("Usage: tf_echo source_frame target_frame [echo_rate]\n\n"); 37 | printf("This will echo the transform from the coordinate frame of the source_frame\n"); 38 | printf("to the coordinate frame of the target_frame. \n"); 39 | printf("Note: This is the transform to get data from target_frame into the source_frame.\n"); 40 | printf("Default echo rate is 1 if echo_rate is not given.\n"); 41 | return -1; 42 | } 43 | 44 | ros::NodeHandle nh; 45 | 46 | double rate_hz; 47 | if (argc == 4) 48 | { 49 | // read rate from command line 50 | rate_hz = atof(argv[3]); 51 | } 52 | else 53 | { 54 | // read rate parameter 55 | ros::NodeHandle p_nh("~"); 56 | p_nh.param("rate", rate_hz, 1.0); 57 | } 58 | if (rate_hz <= 0.0) 59 | { 60 | std::cerr << "Echo rate must be > 0.0\n"; 61 | return -1; 62 | } 63 | ros::Rate rate(1); 64 | 65 | //Instantiate a local listener 66 | echoListener echoListener; 67 | 68 | std::string source_frameid = std::string("base_link"); 69 | std::string target_frameid = std::string("object_16"); 70 | 71 | // Wait for up to one second for the first transforms to become avaiable. 72 | echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0)); 73 | 74 | //Nothing needs to be done except wait for a quit 75 | //The callbacks withing the listener class 76 | //will take care of everything 77 | while(nh.ok()) 78 | { 79 | try 80 | { 81 | tf::StampedTransform echo_transform; 82 | echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform); 83 | std::cout.precision(3); 84 | std::cout.setf(std::ios::fixed,std::ios::floatfield); 85 | std::cout << "At time " << echo_transform.stamp_.toSec() << std::endl; 86 | double yaw, pitch, roll; 87 | echo_transform.getBasis().getRPY(roll, pitch, yaw); 88 | tf::Quaternion q = echo_transform.getRotation(); 89 | tf::Vector3 v = echo_transform.getOrigin(); 90 | std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl; 91 | 92 | //print transform 93 | } 94 | catch(tf::TransformException& ex) 95 | { 96 | std::cout << "Failure at "<< ros::Time::now() << std::endl; 97 | std::cout << "Exception thrown:" << ex.what()<< std::endl; 98 | std::cout << "The current list of frames is:" < 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 | 54 | 55 | 56 | 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 | -------------------------------------------------------------------------------- /src/service_call.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "CR5_Project/ObjectMsg.h" 4 | #include 5 | #include 6 | 7 | void chatterCallback(const CR5_Project::ObjectMsg::ConstPtr& msg) 8 | { 9 | ROS_INFO("Recieved Position :\nx: [%0.3f]\ny: [%0.3f]\nz: [%0.3f]\nrx: [%0.1f]\nry: [%0.1f]\nrz: [%0.1f]", msg->x, msg->y, msg->z, msg->rx, msg->ry, msg->rz); 10 | char cmd[200]; 11 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/MovJ \"{x: %0.3f, y: %0.3f, z: %0.3f, a: %0.3f, b: %0.3f, c: %0.3f}\"'", msg->x, msg->y, msg->z, msg->rx, msg->ry, msg->rz); 12 | system(cmd); 13 | } 14 | 15 | void cmdCallback(const std_msgs::String::ConstPtr& msg) 16 | { 17 | std::string cmd; 18 | cmd = msg->data.c_str(); 19 | if (cmd == "home"){ 20 | char cmd[200]; 21 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/JointMovJ \"{j1: 90.0, j2: 15.0, j3: 83.0, j4: -8.0, j5: -89.0, j6: 0.0}\"'"); 22 | system(cmd); 23 | } 24 | if (cmd == "sleep"){ 25 | char cmd[200]; 26 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/JointMovJ \"{j1: 115.0, j2: -30.0, j3: 135.0, j4: -15.0, j5: -90.0, j6: 0.0}\"'"); 27 | system(cmd); 28 | } 29 | if (cmd == "ClearError"){ 30 | char cmd[200]; 31 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/ClearError'"); 32 | system(cmd); 33 | } 34 | if (cmd == "DisableRobot"){ 35 | char cmd[200]; 36 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/DisableRobot'"); 37 | system(cmd); 38 | } 39 | if (cmd == "EnableRobot"){ 40 | char cmd[200]; 41 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/EnableRobot'"); 42 | system(cmd); 43 | } 44 | if (cmd == "object_not_stay"){ 45 | char cmd_stop[200]; 46 | sprintf(cmd_stop, "gnome-terminal --tab -e 'rosservice call /dobot_bringup/srv/StopScript'"); 47 | system(cmd_stop); 48 | sprintf(cmd_stop, "gnome-terminal --tab -e 'rosservice call /dobot_bringup/srv/EnableRobot'"); 49 | system(cmd_stop); 50 | } 51 | if (cmd == "Point0"){ 52 | char cmd[200]; 53 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/JointMovJ \"{j1:0.0, j2:0.0, j3:0.0, j4:0.0, j5:0.0, j6:0.0}\"'"); 54 | system(cmd); 55 | } 56 | if (cmd == "Point1"){ 57 | char cmd[200]; 58 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/MovJ \"{x: 136.0, y: -550.0, z: 260.0, a: 180.0, b: 0.0, c: 180.0}\"'"); 59 | system(cmd); 60 | } 61 | if (cmd == "Speed100"){ 62 | char cmd_spdJ[200]; 63 | sprintf(cmd_spdJ, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/SpeedJ \"r: 100 \"'"); 64 | system(cmd_spdJ); 65 | } 66 | if (cmd == "Speed50"){ 67 | char cmd_spdJ[200]; 68 | sprintf(cmd_spdJ, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/SpeedJ \"r: 50 \"'"); 69 | system(cmd_spdJ); 70 | } 71 | if (cmd == "inventory"){ 72 | char cmd[200]; 73 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/MovJ \"{x: 264.0, y: -310.0, z: 96.0, a: 180.0, b: 0.0, c: 180.0}\"'"); 74 | system(cmd); 75 | } 76 | if (cmd == "inventory2"){ 77 | char cmd[200]; 78 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call bringup/srv/MovJ \"{x: 0.0, y: -285.0, z: 385.0, a: 0.0, b: 0.0, c: 0.0}\"'"); 79 | system(cmd); 80 | char cmd2[200]; 81 | sprintf(cmd2, "gnome-terminal --tab -e 'rosservice call bringup/srv/MovJ \"{x: 0.0, y: -375.0, z: 330.0, a: -90.0, b: 0.0, c: 0.0}\"'"); 82 | system(cmd2); 83 | } 84 | if (cmd == "SetVac"){ 85 | char cmd_magic[200]; 86 | sprintf(cmd_magic, "gnome-terminal --tab -e 'rosservice call /DobotServer/SetEndEffectorSuctionCup \"enableCtrl: 1 \nsuck: 1 \nisQueued: false\"'"); 87 | system(cmd_magic); 88 | } 89 | if (cmd == "ResetVac"){ 90 | char cmd_magic[200]; 91 | sprintf(cmd_magic, "gnome-terminal --tab -e 'rosservice call /DobotServer/SetEndEffectorSuctionCup \"enableCtrl: 1 \nsuck: 0 \nisQueued: false\"'"); 92 | system(cmd_magic); 93 | } 94 | 95 | else { 96 | char cmd[200]; 97 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call bringup/srv/JointMovJ \"{j1: 0.0, j2: 0.0, j3: 0.0, j4: 0.0, j5: 0.0, j6: 0.0}\"'"); 98 | system(cmd); 99 | } 100 | } 101 | 102 | int main(int argc, char **argv) 103 | { 104 | ros::init(argc, argv, "listener"); 105 | 106 | ros::NodeHandle n; 107 | 108 | ros::Subscriber sub1 = n.subscribe("Object_Position", 100, chatterCallback); 109 | ros::Subscriber sub2 = n.subscribe("Cmd_Talker", 100, cmdCallback); 110 | 111 | // Starting 112 | std::cout << "Started rosrun CR5_Project service_call"; 113 | std::cout << "\n \nSUMMARY"; 114 | std::cout << "\n========"; 115 | std::cout << "\n \nThis program is for using ros service to CR5 Robot. \nStill in development stage."; 116 | std::cout << "\n \nCOMMANDS INPUT"; 117 | std::cout << "\n========"; 118 | 119 | std::cout << "\nInitializing listener node for subscribe commands."; 120 | 121 | char newtmn[100]; 122 | sprintf(newtmn, "gnome-terminal --geometry=90x24+0+0 --window \ --working-directory=/depot --title='Main Control Terminal' --command=\"rosrun CR5_Project main_control\""); 123 | system(newtmn); 124 | 125 | char cmd[200]; 126 | sprintf(cmd, "gnome-terminal --tab -e 'rosservice call dobot_bringup/srv/EnableRobot'"); 127 | system(cmd); 128 | 129 | ros::spin(); 130 | 131 | return 0; 132 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(CR5_Project) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | message_generation 15 | tf 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a exec_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | add_message_files( 53 | FILES 54 | ObjectMsg.msg 55 | ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | generate_messages( 73 | DEPENDENCIES 74 | std_msgs 75 | ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES your_package 109 | CATKIN_DEPENDS roscpp rospy std_msgs message_runtime 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/your_package.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/your_package_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | add_executable(main_control src/main_control.cpp) 155 | target_link_libraries(main_control ${catkin_LIBRARIES}) 156 | #add_dependencies(main_control beginner_tutorials_generate_messages_cpp) 157 | 158 | add_executable(service_call src/service_call.cpp) 159 | target_link_libraries(service_call ${catkin_LIBRARIES}) 160 | #add_dependencies(service_call beginner_tutorials_generate_messages_cpp) 161 | 162 | add_executable(tf_listener src/tf_listener.cpp) 163 | target_link_libraries(tf_listener ${catkin_LIBRARIES}) 164 | 165 | ############# 166 | ## Install ## 167 | ############# 168 | 169 | # all install targets should use catkin DESTINATION variables 170 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 171 | 172 | ## Mark executable scripts (Python etc.) for installation 173 | ## in contrast to setup.py, you can choose the destination 174 | # catkin_install_python(PROGRAMS 175 | # scripts/my_python_script 176 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark executables for installation 180 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 181 | # install(TARGETS ${PROJECT_NAME}_node 182 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 183 | # ) 184 | 185 | ## Mark libraries for installation 186 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 187 | # install(TARGETS ${PROJECT_NAME} 188 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 189 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 190 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 191 | # ) 192 | 193 | ## Mark cpp header files for installation 194 | # install(DIRECTORY include/${PROJECT_NAME}/ 195 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 196 | # FILES_MATCHING PATTERN "*.h" 197 | # PATTERN ".svn" EXCLUDE 198 | # ) 199 | 200 | ## Mark other files for installation (e.g. launch and bag files, etc.) 201 | # install(FILES 202 | # # myfile1 203 | # # myfile2 204 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 205 | # ) 206 | 207 | ############# 208 | ## Testing ## 209 | ############# 210 | 211 | ## Add gtest based cpp test target and link libraries 212 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_your_package.cpp) 213 | # if(TARGET ${PROJECT_NAME}-test) 214 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 215 | # endif() 216 | 217 | ## Add folders to be run by python nosetests 218 | # catkin_add_nosetests(test) 219 | 220 | -------------------------------------------------------------------------------- /src/main_control.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "CR5_Project/ObjectMsg.h" 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include "tf/transform_listener.h" 10 | #include "tf/tf.h" 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace tf; 16 | using namespace ros; 17 | using namespace std; 18 | 19 | #define _USE_MATH_DEFINES 20 | 21 | class echoListener 22 | { 23 | public: 24 | 25 | tf::TransformListener tf; 26 | 27 | //constructor with name 28 | echoListener() 29 | { 30 | 31 | }; 32 | 33 | ~echoListener() 34 | { 35 | 36 | }; 37 | 38 | private: 39 | 40 | }; 41 | 42 | int checkpose (int x, int y, int z) 43 | { 44 | echoListener echoListener; 45 | 46 | std::string source_frameid = std::string("base_link"); 47 | std::string target_eef = std::string("Link6"); 48 | bool check = false; 49 | 50 | while (check == false) { 51 | echoListener.tf.waitForTransform(source_frameid, target_eef, ros::Time(), ros::Duration(0.2)); 52 | tf::StampedTransform echo_transform; 53 | echoListener.tf.lookupTransform(source_frameid, target_eef, ros::Time(), echo_transform); 54 | 55 | //double yaw, pitch, roll; 56 | //echo_transform.getBasis().getRPY(roll, pitch, yaw); 57 | //tf::Quaternion q = echo_transform.getRotation(); 58 | tf::Vector3 v = echo_transform.getOrigin(); 59 | int actual_x,actual_y,actual_z ; 60 | actual_x = v.getX()*1000; 61 | actual_y = v.getY()*1000; 62 | actual_z = v.getZ()*1000; 63 | //std::cout << "Checking: [" << actual_x << ", " << actual_y << ", " << actual_z << "]" << std::endl; 64 | 65 | if (abs(abs(actual_x) - abs(x))<= 5 && abs(abs(actual_y) - abs(y))<= 5 && abs(actual_z - z)<= 5) 66 | {check = true;} 67 | usleep(100000); 68 | } 69 | return 0; 70 | } 71 | 72 | int checkpose_rpy (int x, int y, int z, int rx, int ry, int rz) 73 | { 74 | echoListener echoListener; 75 | 76 | std::string source_frameid = std::string("base_link"); 77 | std::string target_eef = std::string("Link6"); 78 | bool check = false; 79 | 80 | while (check == false) { 81 | echoListener.tf.waitForTransform(source_frameid, target_eef, ros::Time(), ros::Duration(0.2)); 82 | tf::StampedTransform echo_transform; 83 | echoListener.tf.lookupTransform(source_frameid, target_eef, ros::Time(), echo_transform); 84 | 85 | double yaw, pitch, roll; 86 | echo_transform.getBasis().getRPY(roll, pitch, yaw); 87 | tf::Quaternion q = echo_transform.getRotation(); 88 | tf::Vector3 v = echo_transform.getOrigin(); 89 | int actual_x,actual_y,actual_z,actual_rx,actual_ry,actual_rz ; 90 | actual_x = v.getX()*1000; 91 | actual_y = v.getY()*1000; 92 | actual_z = v.getZ()*1000; 93 | actual_rx = roll*180.0/M_PI; 94 | actual_ry = pitch*180.0/M_PI; 95 | actual_rz = yaw*180.0/M_PI; 96 | 97 | std::cout << "Checking: [" << actual_x << ", " << actual_y << ", " << actual_z << ", " << actual_rx << ", " << actual_ry << ", " << actual_rz << "]" << std::endl; 98 | if (abs(abs(actual_x) - abs(x))<= 5 && abs(abs(actual_y) - abs(y))<= 5 && abs(actual_z - z)<= 5 && abs(abs(actual_rx) - abs(rx))<= 5 && abs(abs(actual_ry) - abs(ry))<= 5 && abs(actual_rz - rz)<= 5) 99 | {check = true;} 100 | usleep(100000); 101 | } 102 | return 0; 103 | } 104 | 105 | int checkpose_focus (int x, int y, int z) 106 | { 107 | int at_goal; 108 | echoListener echoListener; 109 | std::string source_frameid = std::string("base_link"); 110 | std::string target_eef = std::string("Link6"); 111 | 112 | echoListener.tf.waitForTransform(source_frameid, target_eef, ros::Time(), ros::Duration(0.2)); 113 | tf::StampedTransform echo_transform; 114 | echoListener.tf.lookupTransform(source_frameid, target_eef, ros::Time(), echo_transform); 115 | tf::Vector3 v = echo_transform.getOrigin(); 116 | int actual_x,actual_y,actual_z ; 117 | 118 | actual_x = v.getX()*1000; 119 | actual_y = v.getY()*1000; 120 | actual_z = v.getZ()*1000; 121 | 122 | if (abs(abs(actual_x) - abs(x))<= 5 && abs(abs(actual_y) - abs(y))<= 5 && abs(actual_z - z)<= 5) 123 | {at_goal = 1;} 124 | else {at_goal = 0;} 125 | 126 | return at_goal; 127 | } 128 | 129 | int checkpose_object (int x, int y, int z, int object_id) 130 | { 131 | int object_stay; 132 | echoListener echoListener; 133 | std::string source_frameid = std::string("base_link"); 134 | std::string target_frameid = std::string("object_1"); 135 | std::string target_frameid2 = std::string("object_2"); 136 | std::string target_frameid3 = std::string("object_3"); 137 | std::string target_frameid4 = std::string("object_4"); 138 | std::string target_frameid5 = std::string("object_5"); 139 | 140 | std::string final_target; 141 | 142 | if (object_id == 1) {final_target = target_frameid;} 143 | if (object_id == 2) {final_target = target_frameid2;} 144 | if (object_id == 3) {final_target = target_frameid3;} 145 | if (object_id == 4) {final_target = target_frameid4;} 146 | if (object_id == 5) {final_target = target_frameid5;} 147 | 148 | ros::NodeHandle n; 149 | ros::Publisher chatter_pub2 = n.advertise("Cmd_Talker", 100); 150 | std_msgs::String msg; 151 | 152 | if (echoListener.tf.waitForTransform(source_frameid, final_target, ros::Time(), ros::Duration(0.8)) == 1) 153 | { 154 | tf::StampedTransform echo_transform; 155 | echoListener.tf.lookupTransform(source_frameid, final_target, ros::Time(), echo_transform); 156 | tf::Vector3 v = echo_transform.getOrigin(); 157 | int actual_x,actual_y,actual_z ; 158 | 159 | actual_x = v.getX()*1000; 160 | actual_y = v.getY()*1000; 161 | actual_z = v.getZ()*1000; 162 | 163 | if (abs(abs(actual_x)-abs(x)) >= 20 || abs(abs(actual_y)-abs(y)) >= 20 || abs(abs(actual_z)-abs(z)) >= 20) 164 | { 165 | //ROS_INFO("Delta X : [%0.3f] Delta Y : [%0.3f] Delta Y : [%0.3f]", abs(abs(actual_x)-abs(x)), abs(abs(actual_y)-abs(y)), abs(abs(actual_z)-abs(z))); 166 | std::stringstream cmd; 167 | ROS_INFO("Object move"); 168 | cmd << "object_not_stay"; 169 | msg.data = cmd.str(); 170 | chatter_pub2.publish(msg); 171 | object_stay = 1; 172 | } 173 | else 174 | { 175 | object_stay = 0; 176 | //ROS_INFO("Delta X : [%0.3f] Delta Y : [%0.3f] Delta Y : [%0.3f]", abs(abs(actual_x)-abs(x)), abs(abs(actual_y)-abs(y)), abs(abs(actual_z)-abs(z))); 177 | } 178 | } 179 | else 180 | { 181 | std::stringstream cmd; 182 | ROS_INFO("Checking object again"); 183 | cmd << "object_not_stay"; 184 | msg.data = cmd.str(); 185 | chatter_pub2.publish(msg); 186 | 187 | if (echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.5)) == 1) 188 | { 189 | object_stay = 1; 190 | } 191 | else 192 | { 193 | ROS_INFO("Object not found"); 194 | object_stay = -1; 195 | } 196 | } 197 | return object_stay; 198 | } 199 | 200 | int checkpose_object_focus (int x, int y, int z, int object_id) 201 | { 202 | int object_stay; 203 | echoListener echoListener; 204 | std::string source_frameid = std::string("base_link"); 205 | std::string target_frameid = std::string("object_1"); 206 | std::string target_frameid2 = std::string("object_2"); 207 | std::string target_frameid3 = std::string("object_3"); 208 | std::string target_frameid4 = std::string("object_4"); 209 | std::string target_frameid5 = std::string("object_5"); 210 | 211 | std::string final_target; 212 | 213 | if (object_id == 1) {final_target = target_frameid;} 214 | if (object_id == 2) {final_target = target_frameid2;} 215 | if (object_id == 3) {final_target = target_frameid3;} 216 | if (object_id == 4) {final_target = target_frameid4;} 217 | if (object_id == 5) {final_target = target_frameid5;} 218 | 219 | ros::NodeHandle n; 220 | ros::Publisher chatter_pub2 = n.advertise("Cmd_Talker", 100); 221 | std_msgs::String msg; 222 | 223 | if (echoListener.tf.waitForTransform(source_frameid, final_target, ros::Time(), ros::Duration(0.3)) == 1) 224 | { 225 | tf::StampedTransform echo_transform; 226 | echoListener.tf.lookupTransform(source_frameid, final_target, ros::Time(), echo_transform); 227 | tf::Vector3 v = echo_transform.getOrigin(); 228 | int actual_x,actual_y,actual_z ; 229 | 230 | actual_x = v.getX()*1000; 231 | actual_y = v.getY()*1000; 232 | actual_z = v.getZ()*1000; 233 | 234 | if (abs(abs(actual_x)-abs(x)) <= 20 || abs(abs(actual_y)-abs(y)) <= 20 || abs(abs(actual_z)-abs(z))<= 20) 235 | { 236 | object_stay = 0; 237 | } 238 | else 239 | { 240 | std::stringstream cmd; 241 | std::cout << "Object move" << std::endl; 242 | cmd << "object_not_stay"; 243 | msg.data = cmd.str(); 244 | chatter_pub2.publish(msg); 245 | object_stay = 1; 246 | } 247 | } 248 | else 249 | { 250 | std::stringstream cmd; 251 | std::cout << "Object move" << std::endl; 252 | cmd << "object_not_stay"; 253 | msg.data = cmd.str(); 254 | chatter_pub2.publish(msg); 255 | 256 | if (echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0)) == 1) 257 | { 258 | object_stay = 1; 259 | } 260 | else 261 | { 262 | std::cout << "Object not found" << std::endl; 263 | object_stay = -1; 264 | } 265 | } 266 | return object_stay; 267 | } 268 | 269 | int picking_object (int object_id, int box_count) 270 | { 271 | ros::NodeHandle n; 272 | ros::Publisher chatter_pub1 = n.advertise("Object_Position", 100); 273 | ros::Publisher chatter_pub2 = n.advertise("Cmd_Talker", 100); 274 | 275 | echoListener echoListener; 276 | std::string source_frameid = std::string("base_link"); 277 | std::string camera_frameid = std::string("camera_link"); 278 | std::string target_frameid = std::string("object_1"); 279 | std::string target_frameid2 = std::string("object_2"); 280 | std::string target_frameid3 = std::string("object_3"); 281 | std::string target_frameid4 = std::string("object_4"); 282 | std::string target_frameid5 = std::string("object_5"); 283 | std::string target_eef = std::string("Link6"); 284 | 285 | std::string final_target; 286 | 287 | if (object_id == 1) {final_target = target_frameid;} 288 | if (object_id == 2) {final_target = target_frameid2;} 289 | if (object_id == 3) {final_target = target_frameid3;} 290 | if (object_id == 4) {final_target = target_frameid4;} 291 | if (object_id == 5) {final_target = target_frameid5;} 292 | 293 | CR5_Project::ObjectMsg msg; 294 | int carry = 0; 295 | int checking = 0; 296 | int place = 0; 297 | int box = box_count; 298 | int box_stack; 299 | 300 | while (place == 0) 301 | { 302 | if (carry == 0 && echoListener.tf.waitForTransform(source_frameid, final_target, ros::Time(), ros::Duration(0.2)) == 1) 303 | { 304 | tf::StampedTransform echo_transform; 305 | echoListener.tf.lookupTransform(source_frameid, final_target, ros::Time(), echo_transform); 306 | tf::Vector3 v = echo_transform.getOrigin(); 307 | 308 | int goal_x = v.getX()*1000; 309 | int goal_y = v.getY()*1000; 310 | int goal_z = v.getZ()*1000; 311 | int goal_rx = 180; 312 | int goal_ry = 0; 313 | int goal_rz = 180; 314 | 315 | int goal_x_cal = goal_x; 316 | int goal_y_cal = goal_y + 50; 317 | int goal_z_cal = 390; 318 | 319 | msg.x = goal_x_cal; 320 | msg.y = goal_y_cal; 321 | msg.z = goal_z_cal; 322 | msg.rx = goal_rx; 323 | msg.ry = goal_ry; 324 | msg.rz = goal_rz; 325 | 326 | ROS_INFO("Moving to x:[%0.3f], y:[%0.3f], z:[%0.3f]", msg.x, msg.y, msg.z); 327 | chatter_pub1.publish(msg); 328 | 329 | while(checkpose_focus(goal_x_cal,goal_y_cal,goal_z_cal) == 0 && checking == 0) 330 | { 331 | int object_state = checkpose_object(goal_x,goal_y,goal_z,object_id); 332 | 333 | //object state 0 = object stay 334 | //object state 1 = object move 335 | //object state -1 = object lost 336 | 337 | if (object_state == 1) 338 | { 339 | checking = 1; 340 | sleep(1); 341 | } 342 | else if (object_state == 0) 343 | { 344 | ROS_INFO(" ~ "); 345 | //usleep(100000); 346 | } 347 | else if (object_state == -1) 348 | { 349 | ROS_INFO("Return to Home Pose "); 350 | std_msgs::String msg; 351 | std::stringstream cmd; 352 | cmd << "object_not_stay"; 353 | msg.data = cmd.str(); 354 | chatter_pub2.publish(msg); 355 | 356 | std::stringstream cmd2; 357 | cmd2 << "home"; 358 | msg.data = cmd2.str(); 359 | chatter_pub2.publish(msg); 360 | checkpose (143,-580,405); 361 | checking = 1; 362 | carry = 0; 363 | sleep(2); 364 | } 365 | } 366 | 367 | if (checkpose_focus(goal_x_cal,goal_y_cal,goal_z_cal) == 1) 368 | { 369 | carry = 1; 370 | usleep(100000); 371 | } 372 | checking = 0; 373 | } 374 | 375 | if (carry == 0 && echoListener.tf.waitForTransform(source_frameid, final_target, ros::Time(), ros::Duration(0.2)) == 0) 376 | { 377 | place = 1; 378 | box_stack == 0; 379 | } 380 | 381 | if (carry == 1) 382 | { 383 | tf::StampedTransform echo_transform; 384 | echoListener.tf.lookupTransform(source_frameid, final_target, ros::Time(), echo_transform); 385 | tf::Vector3 v = echo_transform.getOrigin(); 386 | 387 | int goal_x = v.getX()*1000; 388 | int goal_y = v.getY()*1000; 389 | int goal_z = v.getZ()*1000; 390 | int goal_rx = 180; 391 | int goal_ry = 0; 392 | int goal_rz = 180; 393 | 394 | int goal_x_cal = goal_x; 395 | int goal_y_cal = goal_y + 50; 396 | int goal_z_cal = goal_z + 280; 397 | 398 | msg.x = goal_x_cal; 399 | msg.y = goal_y_cal; 400 | msg.z = goal_z_cal; 401 | msg.rx = goal_rx; 402 | msg.ry = goal_ry; 403 | msg.rz = goal_rz; 404 | 405 | ROS_INFO("Moving to x:[%0.3f], y:[%0.3f], z:[%0.3f]", msg.x, msg.y, msg.z); 406 | chatter_pub1.publish(msg); 407 | 408 | while(checkpose_focus(goal_x_cal,goal_y_cal,goal_z_cal) == 0 && checking == 0) 409 | { 410 | int object_state = checkpose_object(goal_x,goal_y,goal_z,object_id); 411 | 412 | //object state 0 = object stay 413 | //object state 1 = object move 414 | //object state -1 = object lost 415 | 416 | if (object_state == 1){checking = 1; sleep(1);} 417 | else if (object_state == 0){ 418 | ROS_INFO(" ~ "); 419 | //usleep(100000); 420 | } 421 | else if (object_state == -1) 422 | { 423 | ROS_INFO("Return to Home Pose "); 424 | std_msgs::String msg; 425 | std::stringstream cmd; 426 | cmd << "object_not_stay"; 427 | msg.data = cmd.str(); 428 | chatter_pub2.publish(msg); 429 | 430 | std::stringstream cmd2; 431 | cmd2 << "home"; 432 | msg.data = cmd2.str(); 433 | chatter_pub2.publish(msg); 434 | checkpose (143,-580,405); 435 | checking = 1; 436 | carry = 0; 437 | sleep(2); 438 | } 439 | } 440 | if (checkpose_focus(goal_x_cal,goal_y_cal,goal_z_cal) == 1) 441 | { 442 | carry = 2; 443 | usleep(500000); 444 | } 445 | checking = 0; 446 | } 447 | 448 | if (carry == 2){ 449 | 450 | tf::StampedTransform echo_transform; 451 | tf::StampedTransform echo_cam; 452 | echoListener.tf.lookupTransform(source_frameid, final_target, ros::Time(), echo_transform); 453 | echoListener.tf.lookupTransform(camera_frameid, final_target, ros::Time(), echo_cam); 454 | double yaw, pitch, roll; 455 | echo_cam.getBasis().getRPY(roll, pitch, yaw); 456 | tf::Vector3 v = echo_transform.getOrigin(); 457 | 458 | int goal_x = v.getX()*1000; 459 | int goal_y = v.getY()*1000; 460 | int goal_z = v.getZ()*1000; 461 | int goal_roll = roll*180.0/M_PI; 462 | int goal_pitch = pitch*180.0/M_PI; 463 | int goal_yall = yaw*180.0/M_PI; 464 | 465 | int goal_x_cal = goal_x; 466 | int goal_y_cal = goal_y; 467 | int goal_z_cal = goal_z + 54; 468 | int goal_rx_cal = goal_pitch + 180; 469 | int goal_ry_cal = goal_yall - 180; 470 | int goal_rz_cal = goal_roll + 180; 471 | 472 | if (goal_z_cal < 15) {goal_z_cal = 15;} 473 | else if (goal_z_cal >= 15){goal_z_cal = goal_z_cal;} 474 | 475 | msg.x = goal_x_cal; 476 | msg.y = goal_y_cal; 477 | msg.z = goal_z_cal; 478 | msg.rx = goal_rx_cal; 479 | msg.ry = goal_ry_cal; 480 | msg.rz = goal_rz_cal; 481 | 482 | ROS_INFO("Picking Object at x:[%f], y:[%f], z:[%f], rx:[%f], ry:[%f], rz:[%f]", msg.x, msg.y, msg.z, msg.rx, msg.ry, msg.rz); 483 | chatter_pub1.publish(msg); 484 | 485 | usleep(1000000); 486 | std_msgs::String msg; 487 | std::stringstream cmd; 488 | cmd << "SetVac"; 489 | msg.data = cmd.str(); 490 | chatter_pub2.publish(msg); 491 | ROS_INFO("Picked object"); 492 | 493 | checkpose(goal_x_cal,goal_y_cal,goal_z_cal); 494 | 495 | carry = 3; 496 | } 497 | 498 | if (carry == 3) 499 | { 500 | usleep(100000); 501 | 502 | carry = 4; 503 | } 504 | 505 | if (carry == 4) 506 | { 507 | std_msgs::String msg; 508 | std::stringstream cmd; 509 | cmd << "home"; 510 | msg.data = cmd.str(); 511 | chatter_pub2.publish(msg); 512 | checkpose(143,-580,405); 513 | 514 | carry = 5; 515 | } 516 | 517 | if (carry == 5) 518 | { 519 | int goal_x; 520 | int goal_y; 521 | int goal_z; 522 | int z_height; 523 | goal_x = -180; 524 | 525 | if (object_id == 1){goal_y = -710;} 526 | if (object_id == 2){goal_y = -710;} 527 | if (object_id == 3){goal_y = -540;} 528 | if (object_id == 4){goal_y = -540;} 529 | if (object_id == 5){goal_y = -540;} 530 | 531 | goal_z = -70 + (box); 532 | if (goal_z < 50){goal_z = 50;} 533 | else if (goal_z >= 50){goal_z = goal_z;} 534 | 535 | int goal_x_cal = goal_x; 536 | int goal_y_cal = goal_y; 537 | int goal_z_cal = goal_z; 538 | int goal_rx_cal = 180; 539 | int goal_ry_cal = 0; 540 | int goal_rz_cal = 180; 541 | 542 | msg.x = goal_x_cal; 543 | msg.y = goal_y_cal; 544 | msg.z = goal_z_cal; 545 | msg.rx = goal_rx_cal; 546 | msg.ry = goal_ry_cal; 547 | msg.rz = goal_rz_cal; 548 | 549 | ROS_INFO("Placing Object at x:[%f], y:[%f], z:[%f], rx:[%f], ry:[%f], rz:[%f]", msg.x, msg.y, msg.z, msg.rx, msg.ry, msg.rz); 550 | chatter_pub1.publish(msg); 551 | checkpose(goal_x_cal,goal_y_cal,goal_z_cal); 552 | 553 | std_msgs::String msg; 554 | std::stringstream cmd; 555 | cmd << "ResetVac"; 556 | msg.data = cmd.str(); 557 | chatter_pub2.publish(msg); 558 | ROS_INFO("Placed object"); 559 | usleep(200000); 560 | 561 | carry = 6; 562 | } 563 | 564 | if (carry == 6) 565 | { 566 | int goal_x; 567 | int goal_y; 568 | goal_x = -180; 569 | 570 | if (object_id == 1){goal_y = -710;} 571 | if (object_id == 2){goal_y = -710;} 572 | if (object_id == 3){goal_y = -540;} 573 | if (object_id == 4){goal_y = -540;} 574 | if (object_id == 5){goal_y = -540;} 575 | 576 | int goal_x_cal = goal_x; 577 | int goal_y_cal = goal_y; 578 | int goal_z_cal = 200; 579 | int goal_rx_cal = 180; 580 | int goal_ry_cal = 0; 581 | int goal_rz_cal = 180; 582 | 583 | msg.x = goal_x_cal; 584 | msg.y = goal_y_cal; 585 | msg.z = goal_z_cal; 586 | msg.rx = goal_rx_cal; 587 | msg.ry = goal_ry_cal; 588 | msg.rz = goal_rz_cal; 589 | 590 | chatter_pub1.publish(msg); 591 | checkpose(goal_x_cal,goal_y_cal,goal_z_cal); 592 | usleep(200000); 593 | 594 | carry = 7; 595 | } 596 | 597 | if (carry == 7) 598 | { 599 | ROS_INFO("Go Home"); 600 | 601 | std_msgs::String msg; 602 | std::stringstream cmd; 603 | cmd << "home"; 604 | msg.data = cmd.str(); 605 | chatter_pub2.publish(msg); 606 | checkpose(143,-580,405); 607 | usleep(200000); 608 | 609 | carry = 0; 610 | place = 1; 611 | box_stack = box; 612 | } 613 | } 614 | 615 | ROS_INFO("Picking done"); 616 | return box_stack; 617 | } 618 | 619 | int main(int argc, char **argv) 620 | { 621 | ros::init(argc, argv, "talker"); 622 | 623 | ros::NodeHandle n; 624 | 625 | ros::Publisher chatter_pub1 = n.advertise("Object_Position", 100); 626 | ros::Publisher chatter_pub2 = n.advertise("Cmd_Talker", 100); 627 | 628 | ros::Rate loop_rate(10); 629 | 630 | float position_x,position_y,position_z; 631 | 632 | // Starting 633 | std::cout << "Started rosrun CR5_Project main_control"; 634 | std::cout << "\n \nSUMMARY"; 635 | std::cout << "\n========"; 636 | std::cout << "\n \nThis program use for controlling CR5 Robot, still in development stage."; 637 | std::cout << "\nCan also work with Realsense camera D435i."; 638 | std::cout << "\n \nCOMMANDS INPUT"; 639 | std::cout << "\n========"; 640 | std::cout << "\n* Home"; 641 | std::cout << "\n* Pose"; 642 | std::cout << "\n* EnableRobot"; 643 | std::cout << "\n* DisableRobot"; 644 | std::cout << "\n* ClearError"; 645 | std::cout << "\n* Point0"; 646 | std::cout << "\n* Point1"; 647 | std::cout << "\n* Picking"; 648 | std::cout << "\n* PickMe"; 649 | 650 | //Instantiate a local listener 651 | echoListener echoListener; 652 | 653 | std::cout << "\n \nInitializing echoListenter for Transform (tf).\n \n"; 654 | 655 | std::string source_frameid = std::string("base_link"); 656 | std::string camera_frameid = std::string("camera_link"); 657 | std::string target_frameid = std::string("object_1"); 658 | std::string target_frameid2 = std::string("object_2"); 659 | std::string target_frameid3 = std::string("object_3"); 660 | std::string target_frameid4 = std::string("object_4"); 661 | std::string target_frameid5 = std::string("object_5"); 662 | std::string target_eef = std::string("Link6"); 663 | 664 | ROS_INFO("source_frameid = \"base_link\""); 665 | ROS_INFO("target_eef = \"Link6\""); 666 | ROS_INFO("camera_frameid = \"camera_link\""); 667 | ROS_INFO("target_framid = \"object_1\""); 668 | ROS_INFO("target_framid2 = \"object_2\""); 669 | ROS_INFO("target_framid3 = \"object_3\""); 670 | ROS_INFO("target_framid4 = \"object_4\""); 671 | ROS_INFO("target_framid5 = \"object_5\"\n"); 672 | ROS_INFO("Initializing new console terminal for using ros service."); 673 | 674 | ROS_INFO("Enable Robot"); 675 | 676 | while (ros::ok()) 677 | { 678 | ros::Rate rate(1); 679 | 680 | //----------Simple command-------------// 681 | 682 | CR5_Project::ObjectMsg msg; 683 | std::string cmd; 684 | std::cout << "\nUser input command: "; 685 | std::cin >> cmd; 686 | 687 | if (cmd == "Pose"){ 688 | std::cout << "Input position to publish" << std::endl << "position.x :"; 689 | std::cin >> position_x; 690 | std::cout << "position.y :"; 691 | std::cin >> position_y; 692 | std::cout << "position.z :"; 693 | std::cin >> position_z; 694 | 695 | msg.x = position_x; 696 | msg.y = position_y; 697 | msg.z = position_z; 698 | msg.rx = 0; 699 | msg.ry = 0; 700 | msg.rz = 0; 701 | 702 | chatter_pub1.publish(msg); 703 | } 704 | else if (cmd == "Home"){ 705 | std_msgs::String msg; 706 | std::stringstream cmd; 707 | cmd << "home"; 708 | msg.data = cmd.str(); 709 | chatter_pub2.publish(msg); 710 | ROS_INFO("Moving to Home Pose"); 711 | } 712 | else if (cmd == "Sleep"){ 713 | std_msgs::String msg; 714 | std::stringstream cmd; 715 | cmd << "sleep"; 716 | msg.data = cmd.str(); 717 | chatter_pub2.publish(msg); 718 | ROS_INFO("Moving to Sleep Pose"); 719 | } 720 | else if (cmd == "ClearError"){ 721 | std_msgs::String msg; 722 | std::stringstream cmd; 723 | cmd << "ClearError"; 724 | msg.data = cmd.str(); 725 | chatter_pub2.publish(msg); 726 | ROS_INFO("Published Clear Error"); 727 | } 728 | else if (cmd == "DisableRobot"){ 729 | std_msgs::String msg; 730 | std::stringstream cmd; 731 | cmd << "DisableRobot"; 732 | msg.data = cmd.str(); 733 | chatter_pub2.publish(msg); 734 | ROS_INFO("Published Disable Robot"); 735 | } 736 | else if (cmd == "EnableRobot") 737 | { 738 | std_msgs::String msg; 739 | std::stringstream cmd; 740 | cmd << "EnableRobot"; 741 | msg.data = cmd.str(); 742 | chatter_pub2.publish(msg); 743 | ROS_INFO("Published Enable Robot"); 744 | } 745 | else if (cmd == "Point0") 746 | { 747 | std_msgs::String msg; 748 | std::stringstream cmd; 749 | cmd << "Point0"; 750 | msg.data = cmd.str(); 751 | chatter_pub2.publish(msg); 752 | ROS_INFO("Moving to Point 0"); 753 | } 754 | else if (cmd == "Point1") 755 | { 756 | std_msgs::String msg; 757 | std::stringstream cmd; 758 | cmd << "Point1"; 759 | msg.data = cmd.str(); 760 | chatter_pub2.publish(msg); 761 | ROS_INFO("Moving to Point 1"); 762 | } 763 | else if (cmd == "SetVac") 764 | { 765 | std_msgs::String msg; 766 | std::stringstream cmd; 767 | cmd << "SetVac"; 768 | msg.data = cmd.str(); 769 | chatter_pub2.publish(msg); 770 | ROS_INFO("Set Robot Tool0 (Vacuum)"); 771 | } 772 | else if (cmd == "ResetVac") 773 | { 774 | std_msgs::String msg; 775 | std::stringstream cmd; 776 | cmd << "ResetVac"; 777 | msg.data = cmd.str(); 778 | chatter_pub2.publish(msg); 779 | ROS_INFO("Set Robot Tool0 (Vacuum)"); 780 | } 781 | else if (cmd == "Picking") 782 | { 783 | if (echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(0.2)) == 1) 784 | { 785 | picking_object(1,1); 786 | } 787 | else if (echoListener.tf.waitForTransform(source_frameid, target_frameid2, ros::Time(), ros::Duration(0.2)) == 1) 788 | { 789 | picking_object(2,1); 790 | } 791 | else if (echoListener.tf.waitForTransform(source_frameid, target_frameid3, ros::Time(), ros::Duration(0.2)) == 1) 792 | { 793 | picking_object(3,1); 794 | } 795 | else if (echoListener.tf.waitForTransform(source_frameid, target_frameid4, ros::Time(), ros::Duration(0.2)) == 1) 796 | { 797 | picking_object(4,1); 798 | } 799 | else if (echoListener.tf.waitForTransform(source_frameid, target_frameid5, ros::Time(), ros::Duration(0.2)) == 1) 800 | { 801 | picking_object(5,1); 802 | } 803 | } 804 | else if (cmd == "PickMe") 805 | { 806 | int box1 = 0; 807 | int box2 = 0; 808 | int box3 = 0; 809 | int box4 = 0; 810 | int box5 = 0; 811 | int box_stack; 812 | 813 | while (ros::ok) 814 | { 815 | if (echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(0.2)) == 1) 816 | { 817 | box1 += 25; 818 | box_stack = picking_object(1, box1); 819 | box1 = box1 - 25 + box_stack; 820 | } 821 | else if (echoListener.tf.waitForTransform(source_frameid, target_frameid2, ros::Time(), ros::Duration(0.2)) == 1) 822 | { 823 | box1 += 25; 824 | box_stack = picking_object(2, box1); 825 | box1 = box1 - 25 + box_stack; 826 | } 827 | else if (echoListener.tf.waitForTransform(source_frameid, target_frameid3, ros::Time(), ros::Duration(0.2)) == 1) 828 | { 829 | box2 += 30; 830 | box_stack = picking_object(3, box2); 831 | box2 = box2 - 30 + box_stack; 832 | } 833 | else if (echoListener.tf.waitForTransform(source_frameid, target_frameid4, ros::Time(), ros::Duration(0.2)) == 1) 834 | { 835 | box2 += 25; 836 | box_stack = picking_object(4, box2); 837 | box2 = box2 - 25 + box_stack; 838 | } 839 | else if (echoListener.tf.waitForTransform(source_frameid, target_frameid5, ros::Time(), ros::Duration(0.2)) == 1) 840 | { 841 | box2 += 50; 842 | box_stack = picking_object(5, box2); 843 | box2 = box2 - 50 + box_stack; 844 | } 845 | } 846 | } 847 | 848 | else {std::cout << "\n----- command not found ----- \n";} 849 | } 850 | 851 | ros::spinOnce(); 852 | loop_rate.sleep(); 853 | return 0; 854 | } --------------------------------------------------------------------------------