├── .gitignore ├── CMakeLists.txt ├── README.md ├── include └── formation │ ├── discussion │ ├── desirepositioninfo.h │ └── heartbeatinfo.h │ └── reference │ └── leader_reference.h ├── launch ├── discussion.launch ├── discussion2.launch ├── discussion3.launch └── formation.launch ├── msg └── ChatContent.msg ├── package.xml ├── src ├── discussion │ ├── desirepositioninfo.cpp │ └── heartbeatinfo.cpp ├── formation_node.cpp ├── none_leader_discussion.cpp └── reference │ └── leader_reference.cpp └── srv └── PositionRequest.srv /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | 17 | # Generated by dynamic reconfigure 18 | *.cfgc 19 | /cfg/cpp/ 20 | /cfg/*.py 21 | 22 | # Ignore generated docs 23 | *.dox 24 | *.wikidoc 25 | 26 | # eclipse stuff 27 | .project 28 | .cproject 29 | 30 | # qcreator stuff 31 | CMakeLists.txt.user 32 | 33 | srv/_*.py 34 | *.pcd 35 | *.pyc 36 | qtcreator-* 37 | *.user 38 | 39 | /planning/cfg 40 | /planning/docs 41 | /planning/src 42 | 43 | *~ 44 | 45 | # Emacs 46 | .#* 47 | 48 | # Catkin custom files 49 | CATKIN_IGNORE 50 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(formation) 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 | tf 10 | geometry_msgs 11 | actionlib 12 | nav_msgs 13 | roslaunch 14 | std_msgs 15 | message_generation 16 | ) 17 | roslaunch_add_file_check(launch) 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 and a run_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 38 | ## pulled in transitively but can be declared for certainty nonetheless: 39 | ## * add a build_depend tag for "message_generation" 40 | ## * add a run_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 | ChatContent.msg 55 | ) 56 | 57 | ## Generate services in the 'srv' folder 58 | add_service_files( 59 | FILES 60 | PositionRequest.srv 61 | ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | generate_messages( 72 | DEPENDENCIES 73 | std_msgs # Or other packages containing msgs 74 | ) 75 | 76 | ################################### 77 | ## catkin specific configuration ## 78 | ################################### 79 | ## The catkin_package macro generates cmake config files for your package 80 | ## Declare things to be passed to dependent projects 81 | ## INCLUDE_DIRS: uncomment this if you package contains header files 82 | ## LIBRARIES: libraries you create in this project that dependent projects also need 83 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 84 | ## DEPENDS: system dependencies of this project that dependent projects also need 85 | catkin_package( 86 | LIBRARIES 87 | leader_reference 88 | none_leader_discussion 89 | CATKIN_DEPENDS 90 | roscpp 91 | rospy 92 | tf 93 | geometry_msgs 94 | actionlib 95 | nav_msgs 96 | roslaunch 97 | message_generation 98 | INCLUDE_DIRS include 99 | # DEPENDS system_lib 100 | ) 101 | 102 | ########### 103 | ## Build ## 104 | ########### 105 | 106 | ## Specify additional locations of header files 107 | ## Your package locations should be listed before other locations 108 | include_directories(include) 109 | include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 110 | 111 | ## Here we specify the list of source files, including the output of 112 | ## the previous command which is stored in ``${MOC_FILES}``. 113 | #set(SOURCE_FILES 114 | # src/formation_node.cpp 115 | # src/leader_reference.cpp 116 | # src/heartbeatinfo.cpp 117 | # src/desirepositioninfo.cpp 118 | # ${MOC_FILES} 119 | #) 120 | 121 | ## An rviz plugin is just a shared library, so here we declare the 122 | ## library to be called ``${PROJECT_NAME}`` (which is 123 | ## "learning_tf", or whatever your version of this project 124 | ## is called) and specify the list of source files we collected above 125 | ## in ``${SOURCE_FILES}``. 126 | add_library(leader_reference 127 | src/reference/leader_reference.cpp 128 | ) 129 | 130 | add_library(none_leader_discussion 131 | src/discussion/desirepositioninfo.cpp 132 | src/discussion/heartbeatinfo.cpp 133 | ) 134 | target_link_libraries(leader_reference none_leader_discussion) 135 | ## Declare a cpp executable 136 | # add_executable(learning_tf_node src/learning_tf_node.cpp) 137 | add_executable(formation_node src/formation_node.cpp) 138 | add_dependencies(formation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | target_link_libraries(formation_node ${catkin_LIBRARIES} leader_reference none_leader_discussion) 140 | 141 | add_executable(discussion_node src/none_leader_discussion.cpp) 142 | add_dependencies(discussion_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | target_link_libraries(discussion_node ${catkin_LIBRARIES} none_leader_discussion) 144 | 145 | ## Add cmake target dependencies of the executable/library 146 | ## as an example, message headers may need to be generated before nodes 147 | # add_dependencies(learning_tf_node learning_tf_generate_messages_cpp) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(learning_tf_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Testing ## 156 | ############# 157 | 158 | ## Add gtest based cpp test target and link libraries 159 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_learning_tf.cpp) 160 | # if(TARGET ${PROJECT_NAME}-test) 161 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 162 | # endif() 163 | 164 | ## Add folders to be run by python nosetests 165 | # catkin_add_nosetests(test) 166 | 167 | ## Link the library with whatever Qt libraries have been defined by 168 | ## the ``find_package(Qt4 ...)`` line above, and with whatever libraries 169 | ## catkin has included. 170 | ## 171 | ## Although this puts "rviz_plugin_tutorials" (or whatever you have 172 | ## called the project) as the name of the library, cmake knows it is a 173 | ## library and names the actual file something like 174 | ## "librviz_plugin_tutorials.so", or whatever is appropriate for your 175 | ## particular OS. 176 | 177 | ############# 178 | ## Install ## 179 | ############# 180 | 181 | # all install targets should use catkin DESTINATION variables 182 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 183 | 184 | ## Mark executable scripts (Python etc.) for installation 185 | ## in contrast to setup.py, you can choose the destination 186 | # install(PROGRAMS 187 | # scripts/my_python_script 188 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 189 | # ) 190 | 191 | ## Mark executables and/or libraries for installation 192 | install(TARGETS formation_node discussion_node 193 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 194 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 195 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 196 | ) 197 | 198 | install(DIRECTORY include/formation/ 199 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 200 | ) 201 | 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Swarm Robot Formation 2 |   本项目需要使用到[move_base](http://wiki.ros.org/move_base)导航包，请预先安装 3 | ## 下面是本项目的使用教程 4 | #### 一、Arduino的代码 5 | 1、下载turtlebot的Arduino代码：
6 |   https://github.com/SwarmRoboticsSUSTechOPAL/turtlebot3_arduino_sustc
7 | 8 | 2、根据如下[教程](http://emanual.robotis.com/docs/en/platform/turtlebot3/opencr1_0_software_setup/#usb-port-settings)配置环境:
9 |   http://emanual.robotis.com/docs/en/platform/turtlebot3/opencr1_0_software_setup/#usb-port-settings
10 | 11 | 3、在代码中搜索“SUSTC”，修改相应行的代码，如：
12 | ``` 13 | joint_states.header.frame_id = " robot1_base_footprint "; // "base_footprint" SUSTC 14 | ``` 15 |   如果是2号机器人就将"robot1_base_footprint"改成"robot2_base_footprint"
16 | 17 | 4、用Arduino将代码烧录到opencr板子上
18 | 19 | #### 二、ROS的代码 20 | 1、下载turtlebot的ROS代码：
21 |   https://github.com/SwarmRoboticsSUSTechOPAL/swarm_robot_formation
22 |   https://github.com/SwarmRoboticsSUSTechOPAL/tuttlebot3_sustc
23 | 24 | 2、如果想使用QTCreator进行ROS开发，可以按照如下方式配置QTCreator开发环境：
25 |   https://ros-industrial.github.io/ros_qtc_plugin/_source/How-to-Install-Users.html
26 | 27 | 3、在工程中搜索"sustc"，对出现的"robot*"进行修改就行了，如：
28 |   如果是2号机器人就将改成"robot2"
29 | 30 | 4、将代码放到树莓派上进行编译，直接用catkin_make可能会出现资源不够导致卡住的问题，可以用catkin_make –j2进行编译，多试几次就能编译过了
31 | 32 | #### 三、运行 33 | 1、配置环境export TURTLEBOT3_MODEL=burger
34 | 35 | 2、启动turtlebot硬件(in robot)：
36 |   roslaunch turtlebot3_bringup turtlebot3_robot.launch
37 | 38 | 3、启动navigation(in robot):
39 |   export TURTLEBOT3_MODEL=burger
40 |   roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml
41 |   本项目使用的地图位于[maps](https://github.com/SwarmRoboticsSUSTechOPAL/turtlebot_maps)中，每个地图包含.pgm和.yaml两个文件，将两个文件同时拷到home目录下并修改名称为"map"即可使用
42 | Note：如果想使用自己环境下的地方，请使用如下slam包自行构建：
43 |   https://github.com/ROBOTIS-GIT/turtlebot3/tree/master/turtlebot3_slam
44 | 45 | 4、启动formation
46 |   roslaunch formation formation.launch
47 | 48 | 5、启动rviz:
49 |   export TURTLEBOT3_MODEL=burger
50 |   rosrun rviz rviz -d `rospack find turtlebot3_navigation`/rviz/turtlebot3_nav.rviz
51 | 52 | 53 | #### 四、下面所列出的是对应于不同编号机器需要修改的文件（在工程中搜索SUSTC即可找到） 54 | 1、编队启动文件：
55 |   formation formation.launch 56 | 57 | 2、turtlebot硬件相关启动文件：
58 |   turtlebot3 turtlebot3_robot.launch（turtlebot3_core.launch、turtlebot3_lidar.launch） 59 | 60 | 3、turtlebot描述文件：
61 |   turtlebot3 turtlebot3_burger.urdf.xacro（turtlebot3_burger.gazebo.xacro） 62 | 63 | 4、导航启动及配置相关文件：
64 |   turtlebot3 amcl.launch.xml
65 |   turtlebot3 turtlebot3_navigation.launch(turtlebot3_remote.launch(description.launch.xml 66 | (turtlebot3_burger.urdf.xacro)))
67 |   turtlebot3 global_costmap_params.yaml
68 |   turtlebot3 local_costmap_params.yaml
69 | 70 | 5、编队导航的rviz配置文件（不需要修改文件，可以在rivz界面上配置）：
71 |   turtlebot3 turtlebot3_nav.rviz 72 | 73 | 6、下面的一些启动文件是gazebo仿真相关的（如果想在仿真环境下配合导航、slam包、编队包使用，需要修改一下机器人的编号）：
74 |   turtlebot3_simulations turtlebot3_world.launch
75 |   turtlebot3_simulations turtlebot3_gazebo_rviz.launch（turtlebot3_gazebo_model.rviz、description.launch.xml）启动rviz
76 | 77 |   turtlebot3_simulations turtlebot3_model.launch（model.rviz）
78 | -------------------------------------------------------------------------------- /include/formation/discussion/desirepositioninfo.h: -------------------------------------------------------------------------------- 1 | #ifndef DESIREPOSITIONINFO_H 2 | #define DESIREPOSITIONINFO_H 3 | 4 | namespace formation { 5 | 6 | class DesirePositionInfo 7 | { 8 | public: 9 | DesirePositionInfo(); 10 | DesirePositionInfo(int position, int time); 11 | void setPosition(int position); 12 | int getPosition(); 13 | void setTime(int time); 14 | int getTime(); 15 | 16 | private: 17 | int Position; 18 | int Time; 19 | }; 20 | 21 | } /* namespace formation */ 22 | 23 | #endif // DESIREPOSITIONINFO_H 24 | -------------------------------------------------------------------------------- /include/formation/discussion/heartbeatinfo.h: -------------------------------------------------------------------------------- 1 | #ifndef HEARTBEAT_H 2 | #define HEARTBEAT_H 3 | 4 | namespace formation { 5 | 6 | class HeartBeatInfo 7 | { 8 | public: 9 | HeartBeatInfo(); 10 | HeartBeatInfo(int robotID, int time, bool timeout); 11 | void setRobotID(int robotID); 12 | int getRobotID(); 13 | void setTime(int time); 14 | int getTime(); 15 | void setTimeout(bool timeout); 16 | bool getTimeout(); 17 | 18 | private: 19 | int RobotID; 20 | int Time; 21 | bool Timeout; 22 | }; 23 | 24 | } /* namespace formation */ 25 | #endif // HEARTBEAT_H 26 | -------------------------------------------------------------------------------- /include/formation/reference/leader_reference.h: -------------------------------------------------------------------------------- 1 | #ifndef LEADER_RERENCE_H_ 2 | #define LEADER_RERENCE_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace formation { 12 | 13 | /** 14 | */ 15 | enum Formation { Line, Wedge, Column, Diamond}; 16 | 17 | /** 18 | */ 19 | class LeaderRerence { 20 | public: 21 | LeaderRerence(tf::TransformListener* tf, Formation pF, double pDS); 22 | ~LeaderRerence() {} 23 | bool getDesiredPosition(geometry_msgs::PoseStamped& desiredPosition, int position, 24 | int leader_id, geometry_msgs::PoseStamped globalPlanPose); 25 | 26 | protected: 27 | Formation f; 28 | double desiredSpaceing; 29 | 30 | private: 31 | bool getRobotPose(tf::Stamped& global_pose, int leader_id) const; 32 | bool tramformToOdom(tf::Stamped& orign_pose, geometry_msgs::PoseStamped& output_pose) const; 33 | bool transformToMap(geometry_msgs::PoseStamped& globalPlanPose, tf::Stamped& output_pose_map); 34 | tf::TransformListener* tfListener_; 35 | ros::NodeHandle nh_; 36 | std::string mNameSpace_; 37 | }; 38 | 39 | } /* namespace formation */ 40 | #endif /* LEADER_RERENCE_H_ */ 41 | -------------------------------------------------------------------------------- /launch/discussion.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/discussion2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/discussion3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/formation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /msg/ChatContent.msg: -------------------------------------------------------------------------------- 1 | string msg_type 2 | uint32 robot_id 3 | uint32 desire_position 4 | uint32 talk_to #use robot id 5 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | formation 3 | 1.0.0 4 | 5 | 6 | This package provides implementations of the formation in swarm robots. 7 | 8 | 9 | LiGao 10 | LiGao 11 | BSD 12 | 13 | catkin 14 | 15 | cmake_modules 16 | std_msgs 17 | nav_msgs 18 | roscpp 19 | tf 20 | pluginlib 21 | angles 22 | geometry_msgs 23 | pcl_conversions 24 | pcl_ros 25 | eigen 26 | message_generation 27 | roslaunch 28 | actionlib 29 | 30 | std_msgs 31 | nav_msgs 32 | roscpp 33 | tf 34 | rospy 35 | pluginlib 36 | angles 37 | geometry_msgs 38 | pcl_ros 39 | eigen 40 | message_runtime 41 | move_base 42 | actionlib 43 | roslaunch 44 | message_generation 45 | 46 | -------------------------------------------------------------------------------- /src/discussion/desirepositioninfo.cpp: -------------------------------------------------------------------------------- 1 | #include "formation/discussion/desirepositioninfo.h" 2 | 3 | using namespace formation; 4 | 5 | DesirePositionInfo::DesirePositionInfo() 6 | { 7 | 8 | } 9 | 10 | DesirePositionInfo::DesirePositionInfo(int position, int time) 11 | { 12 | this->Position = position; 13 | this->Time = time; 14 | } 15 | 16 | void DesirePositionInfo::setPosition(int position) 17 | { 18 | this->Position = position; 19 | } 20 | 21 | int DesirePositionInfo::getPosition() 22 | { 23 | return this->Position; 24 | } 25 | 26 | void DesirePositionInfo::setTime(int time) 27 | { 28 | this->Time = time; 29 | } 30 | 31 | int DesirePositionInfo::getTime() 32 | { 33 | return this->Time; 34 | } 35 | -------------------------------------------------------------------------------- /src/discussion/heartbeatinfo.cpp: -------------------------------------------------------------------------------- 1 | #include "formation/discussion/heartbeatinfo.h" 2 | 3 | using namespace formation; 4 | 5 | HeartBeatInfo::HeartBeatInfo() 6 | { 7 | 8 | } 9 | 10 | HeartBeatInfo::HeartBeatInfo(int robotID, int time, bool timeout) 11 | { 12 | this->RobotID = robotID; 13 | this->Time = time; 14 | this->Timeout = timeout; 15 | } 16 | 17 | void HeartBeatInfo::setRobotID(int robotID) 18 | { 19 | this->RobotID = robotID; 20 | } 21 | 22 | int HeartBeatInfo::getRobotID() 23 | { 24 | return this->RobotID; 25 | } 26 | 27 | void HeartBeatInfo::setTime(int time) 28 | { 29 | this->Time = time; 30 | } 31 | 32 | int HeartBeatInfo::getTime() 33 | { 34 | return this->Time; 35 | } 36 | 37 | void HeartBeatInfo::setTimeout(bool timeout) 38 | { 39 | this->Timeout = timeout; 40 | } 41 | 42 | bool HeartBeatInfo::getTimeout() 43 | { 44 | return this->Timeout; 45 | } 46 | -------------------------------------------------------------------------------- /src/formation_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "formation/reference/leader_reference.h" 4 | #include "formation/PositionRequest.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace formation; 18 | 19 | bool g_start_formation_flag = false; 20 | geometry_msgs::PoseStamped g_leader_plan_pose; 21 | actionlib::SimpleActionClient *g_ac = NULL; 22 | ros::Publisher g_sendplan_pub; 23 | ros::ServiceClient g_clear_map_sc; 24 | bool g_isleader = false; 25 | 26 | void startCommandReceived(const geometry_msgs::PoseStamped& msg) 27 | { 28 | ROS_INFO("Received goal."); 29 | if(g_isleader) 30 | { 31 | move_base_msgs::MoveBaseGoal goal; 32 | goal.target_pose.header = msg.header; 33 | goal.target_pose.pose = msg.pose; 34 | // send goal to reach if this node is the leader 35 | g_ac->sendGoal(goal); 36 | } 37 | } 38 | 39 | /* 40 | * In this function, we can get the global path of leader. 41 | * So, the pose at position 0 is the begining point of the path. 42 | * And, we can get Yaw through the Quaternion of orientation. The scope is 0~pi and 0~-pi.Positive of X define as zore. 43 | */ 44 | void getGlobalPlanOfLeader(const nav_msgs::Path& gui_path) 45 | { 46 | ROS_INFO("Receive global plan of leader."); 47 | g_start_formation_flag = true; 48 | int sizeOfPathPose = (int)gui_path.poses.size(); 49 | // ROS_INFO("gui_path.poses.size(): %d", sizeOfPathPose); 50 | if(sizeOfPathPose >= 40) 51 | { 52 | g_leader_plan_pose = gui_path.poses[(gui_path.poses.size()-1)/2]; 53 | } 54 | else 55 | { 56 | g_leader_plan_pose = gui_path.poses[gui_path.poses.size()-1]; 57 | } 58 | // ROS_INFO("gui_path.poses[end]: %f %f %f, %f %f %f %f", g_leader_plan_pose.pose.position.x, g_leader_plan_pose.pose.position.y, g_leader_plan_pose.pose.position.z, 59 | // g_leader_plan_pose.pose.orientation.x, g_leader_plan_pose.pose.orientation.y, g_leader_plan_pose.pose.orientation.z, g_leader_plan_pose.pose.orientation.w); 60 | 61 | // ROS_INFO("yawOfPath_odom: %f", tf::getYaw(g_leader_plan_pose.pose.orientation)); 62 | } 63 | 64 | void getSelfGlobalPlan(const nav_msgs::Path& gui_path) 65 | { 66 | if(g_isleader) 67 | { 68 | g_sendplan_pub.publish(gui_path); 69 | } 70 | } 71 | 72 | void shutdown(int sig) 73 | { 74 | ros::Duration(1).sleep(); // sleep for one second 75 | ROS_INFO("formation node ended!"); 76 | if(g_ac != NULL) 77 | { 78 | delete g_ac; 79 | g_ac = NULL; 80 | } 81 | ros::shutdown(); 82 | } 83 | 84 | void clearCostMapTimer(const ros::TimerEvent& event) 85 | { 86 | std_srvs::Empty::Request req_clearCostmap; 87 | std_srvs::Empty::Response res_clearCostmap; 88 | g_clear_map_sc.call(req_clearCostmap, res_clearCostmap); 89 | } 90 | 91 | int main(int argc, char **argv) 92 | { 93 | ros::init(argc, argv, "formation_node"); 94 | ros::NodeHandle nh; 95 | tf::TransformListener tfListener(ros::Duration(10)); 96 | ros::Rate loop_rate(40); 97 | ROS_INFO("in the node!"); 98 | std::string ns = nh.getNamespace(); 99 | ns = ns.substr(1, ns.size() - 1); 100 | signal(SIGINT, shutdown); 101 | formation::LeaderRerence leaderRerence(&tfListener, formation::Line, 0.3); 102 | std::stringstream leader_id_stream; 103 | int leader_id; 104 | int position = -1; 105 | 106 | std_srvs::Empty::Request req_clearCostmap; 107 | std_srvs::Empty::Response res_clearCostmap; 108 | formation::PositionRequest::Request req_position; 109 | formation::PositionRequest::Response res_position; 110 | 111 | g_ac = new actionlib::SimpleActionClient(ns + "/move_base", true); 112 | while(!g_ac->waitForServer(ros::Duration(5))) 113 | { 114 | ROS_WARN("Can't connect to move base server, %s!", ns.c_str()); 115 | ros::spinOnce(); 116 | } 117 | ROS_INFO("Connected to move base server, %s!", ns.c_str()); 118 | ros::Subscriber getGoalSub = nh.subscribe("/goal", 100, &startCommandReceived); 119 | 120 | ros::Subscriber get_selfpaln_sub = nh.subscribe(ns + "/move_base/DWAPlannerROS/global_plan", 100, &getSelfGlobalPlan); 121 | ros::Subscriber get_leaderpaln_sub = nh.subscribe("/leader_plan", 100, getGlobalPlanOfLeader); 122 | g_sendplan_pub = nh.advertise("/leader_plan", 100); 123 | 124 | g_clear_map_sc = nh.serviceClient(ns + "/move_base/clear_costmaps"); 125 | ros::ServiceClient get_position_sc = nh.serviceClient(ns + "/position_requestion"); 126 | 127 | ros::Timer timer = nh.createTimer(ros::Duration(5), clearCostMapTimer); 128 | 129 | ros::Duration(1).sleep(); 130 | //request the position by argument 131 | get_position_sc.call(req_position, res_position); 132 | if(leader_id != res_position.leader_id) 133 | { 134 | g_start_formation_flag = false; 135 | } 136 | leader_id = res_position.leader_id; 137 | leader_id_stream.str(""); 138 | leader_id_stream<sendGoal(goal); 184 | bool finish_within_time = g_ac->waitForResult(ros::Duration(1)); 185 | 186 | if(!finish_within_time) 187 | { 188 | g_clear_map_sc.call(req_clearCostmap, res_clearCostmap); 189 | g_ac->cancelGoal(); 190 | ROS_INFO("Time out achieving goal."); 191 | } 192 | else 193 | { 194 | if(g_ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 195 | { 196 | ROS_INFO("Goal succeeded!"); 197 | } 198 | else 199 | { 200 | ROS_INFO("The base failed for some reason."); 201 | } 202 | } 203 | ROS_INFO("g_ac.getState().getText(): %s", g_ac->getState().toString().c_str()); 204 | } 205 | ros::spinOnce(); 206 | loop_rate.sleep(); 207 | } 208 | 209 | if(g_ac != NULL) 210 | { 211 | delete g_ac; 212 | g_ac = NULL; 213 | } 214 | return 0; 215 | } 216 | -------------------------------------------------------------------------------- /src/none_leader_discussion.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "formation/ChatContent.h" 4 | #include "formation/reference/leader_reference.h" 5 | #include "formation/discussion/desirepositioninfo.h" 6 | #include "formation/discussion/heartbeatinfo.h" 7 | #include "formation/PositionRequest.h" 8 | 9 | using namespace formation; 10 | 11 | const std::string SCRAMBLE = "scramble"; 12 | const std::string HEARTBEAT = "heartbeat"; 13 | const std::string REJECT = "reject"; 14 | const int CANNOT_FIND_VACANCY = -1; 15 | const int LATEST_TIME = 10; 16 | 17 | std::vector g_desirePositions; 18 | std::vector g_grabbedPositions; 19 | int g_scramble_position; 20 | int g_desire_position; 21 | uint32_t g_my_id; 22 | int g_scramble_count = 0; 23 | const HeartBeatInfo g_initial_info(-1, 0, false); 24 | 25 | ros::Publisher g_chatter_pub; 26 | 27 | /* 28 | * Insert the heartbeat info into grabbed table. 29 | * If has not enought space, than resize the grabbed table. 30 | */ 31 | void insertToTable(int position, HeartBeatInfo info) 32 | { 33 | if(g_grabbedPositions.size() <= position) 34 | { 35 | g_grabbedPositions.resize(position+1, g_initial_info); 36 | } 37 | g_grabbedPositions[position] = info; 38 | } 39 | 40 | /* 41 | * Update the heartbeat time of the grabbed table for each position. 42 | * Set the robot id with the num -1, if time is out. 43 | */ 44 | void updateTime() 45 | { 46 | HeartBeatInfo info; 47 | for(int i = 0; i < g_grabbedPositions.size(); i++) 48 | { 49 | info = g_grabbedPositions[i]; 50 | if(info.getRobotID() != -1) 51 | { 52 | info.setTime(info.getTime() + 1); 53 | if(info.getTime() > LATEST_TIME) 54 | { 55 | info.setTimeout(true); 56 | info.setRobotID(-1); 57 | } 58 | g_grabbedPositions[i] = info; 59 | } 60 | } 61 | } 62 | 63 | /* 64 | * Find a scramble position.Search from position 0. 65 | * This function is used only when the size of desired positions is 0. 66 | * */ 67 | int getScramblePosition() 68 | { 69 | int position = 0; //start form position 0 70 | for(int i = 0; i < g_grabbedPositions.size(); i++) 71 | { 72 | HeartBeatInfo info = g_grabbedPositions[i]; 73 | if(info.getRobotID() != -1) 74 | { 75 | position = i + 1; 76 | } 77 | else 78 | { 79 | break; 80 | } 81 | } 82 | return position; 83 | } 84 | 85 | /* 86 | * Find a scramble position when has vacancy in front. 87 | * This function is used only when the size of desried position is 1. 88 | * Search in front self position. 89 | * */ 90 | int findVacancy() 91 | { 92 | DesirePositionInfo dinfo = *(g_desirePositions.begin()); 93 | int vacancyPosition = dinfo.getPosition() - 1; 94 | HeartBeatInfo info = g_grabbedPositions[vacancyPosition]; 95 | 96 | //Search in front self position 97 | for(; vacancyPosition >= 0; vacancyPosition --) 98 | { 99 | info = g_grabbedPositions[vacancyPosition]; 100 | if(info.getRobotID() != -1) 101 | { 102 | break; 103 | } 104 | } 105 | ROS_INFO("dinfo.getPosition() - 1:%d", dinfo.getPosition() - 1); 106 | ROS_INFO("vacancyPosition:%d", vacancyPosition); 107 | ROS_INFO("g_scramble_position:%d", g_scramble_position); 108 | 109 | // If "dinfo.getPosition() - 1 == vacancyPosition", there are not vacancy positions in front of self position. 110 | // "g_scramble_position != -1 && g_scramble_position ==vacancyPosition", we don't scranble the same position last time scrambled. 111 | if(dinfo.getPosition() - 1 == vacancyPosition || 112 | (g_scramble_position != -1 && g_scramble_position ==vacancyPosition)) 113 | { 114 | return -1; 115 | } 116 | else 117 | { 118 | // Because the use of "vacancyPosition --" in "for()" circulation. 119 | return vacancyPosition + 1; 120 | } 121 | } 122 | 123 | void receiver(const formation::ChatContent& receivedMsg) 124 | { 125 | if(receivedMsg.msg_type == SCRAMBLE) 126 | { 127 | //Heared self message 128 | if(receivedMsg.robot_id == g_my_id && g_scramble_count == 0 && receivedMsg.desire_position == g_scramble_position) 129 | { 130 | //Authorized, and update desired position table 131 | DesirePositionInfo dinfo(receivedMsg.desire_position, 0); 132 | // Insert desired position into the zore position of the table. 133 | g_desirePositions.insert(g_desirePositions.begin(), 1, dinfo); 134 | //write into grabbed position table 135 | HeartBeatInfo info; 136 | info.setRobotID(receivedMsg.robot_id); 137 | info.setTime(0); 138 | info.setTimeout(false); 139 | insertToTable(receivedMsg.desire_position, info); 140 | } 141 | g_scramble_count = 1; 142 | 143 | //send reject message 144 | for(int i = 0; i< g_desirePositions.size(); i++) 145 | { 146 | DesirePositionInfo dinfo = g_desirePositions[i]; 147 | if(dinfo.getPosition() == receivedMsg.desire_position && receivedMsg.robot_id != g_my_id) 148 | { 149 | //REJECT 150 | formation::ChatContent sendMsg; 151 | sendMsg.msg_type = REJECT; 152 | sendMsg.robot_id = g_my_id; 153 | sendMsg.desire_position = dinfo.getPosition(); 154 | sendMsg.talk_to = receivedMsg.robot_id; 155 | g_chatter_pub.publish(sendMsg); 156 | } 157 | } 158 | } 159 | else if(receivedMsg.msg_type == HEARTBEAT) 160 | { 161 | //record update table 162 | HeartBeatInfo info; 163 | info.setRobotID(receivedMsg.robot_id); 164 | info.setTime(0); 165 | info.setTimeout(false); 166 | insertToTable(receivedMsg.desire_position, info); 167 | } 168 | else if(receivedMsg.msg_type == REJECT) 169 | { 170 | //write heartbeat info into grabbed table 171 | HeartBeatInfo info; 172 | info.setRobotID(receivedMsg.robot_id); 173 | info.setTime(0); 174 | info.setTimeout(false); 175 | insertToTable(receivedMsg.desire_position, info); 176 | 177 | if(receivedMsg.robot_id != g_my_id && receivedMsg.talk_to == g_my_id) 178 | { 179 | //update desired position table, delete the reject position 180 | DesirePositionInfo dinfo; 181 | for(int i = 0; i < g_desirePositions.size(); i++) 182 | { 183 | dinfo = g_desirePositions[i]; 184 | if(dinfo.getPosition() == receivedMsg.desire_position) 185 | { 186 | g_desirePositions.erase(g_desirePositions.begin() + i); 187 | } 188 | } 189 | 190 | if(g_desirePositions.size() == 0) 191 | { 192 | //If the size of desired position is 0, we scramble position again. 193 | formation::ChatContent sendMsg; 194 | //search teble 195 | g_scramble_position = getScramblePosition(); 196 | sendMsg.msg_type = SCRAMBLE; 197 | sendMsg.robot_id = g_my_id; 198 | sendMsg.desire_position = g_scramble_position; 199 | g_chatter_pub.publish(sendMsg); 200 | g_scramble_count = 0; 201 | } 202 | else if(g_desirePositions.size() == 1) 203 | { 204 | //Reset the heartbeat info of the only one desired position 205 | DesirePositionInfo dinfo = g_desirePositions[0]; 206 | HeartBeatInfo info = g_grabbedPositions[dinfo.getPosition()]; 207 | info.setTime(0); 208 | info.setTimeout(false); 209 | insertToTable(dinfo.getPosition(), info); 210 | } 211 | 212 | //Reset the timeout info of the frist desired position, 213 | //we don't want to timeing the frist desired position. 214 | if(g_desirePositions.size() > 0) 215 | { 216 | DesirePositionInfo dinfo = g_desirePositions[0]; 217 | dinfo.setTime(dinfo.getTime() + 1); 218 | g_desirePositions[0] = dinfo; 219 | } 220 | } 221 | } 222 | } 223 | 224 | /* 225 | * In this function 226 | * 1.we timing the heartbeat info, 227 | * 2.delete the time timeout desired position, 228 | * 3.scramble vacancy position, 229 | * */ 230 | void timerCallback(const ros::TimerEvent& event) 231 | { 232 | //update the timing of heartbeat table 233 | updateTime(); 234 | 235 | if(g_desirePositions.size() > 0) 236 | { 237 | for(int i = g_desirePositions.size() - 1; i > -1; i--) 238 | { 239 | //HEARTBEAT 240 | formation::ChatContent sendMsg; 241 | DesirePositionInfo dinfo = g_desirePositions[i]; 242 | sendMsg.msg_type = HEARTBEAT; 243 | sendMsg.robot_id = g_my_id; 244 | sendMsg.desire_position = dinfo.getPosition(); 245 | g_chatter_pub.publish(sendMsg); 246 | //update the time of desire position 247 | if(i > 0) 248 | { 249 | dinfo.setTime(dinfo.getTime() + 1); 250 | if(dinfo.getTime() > LATEST_TIME) 251 | { 252 | g_desirePositions.erase(g_desirePositions.begin() + i); 253 | } 254 | else 255 | { 256 | g_desirePositions[i] = dinfo; 257 | } 258 | } 259 | } 260 | 261 | //search table, find vacancy position and scramble 262 | if(g_desirePositions.size() == 1) 263 | { 264 | g_scramble_position = findVacancy(); 265 | ROS_INFO("g_scramble_position:%d", g_scramble_position); 266 | if(g_scramble_position > -1) 267 | { 268 | //SCRAMBLE 269 | formation::ChatContent sendMsg; 270 | sendMsg.msg_type = SCRAMBLE; 271 | sendMsg.robot_id = g_my_id; 272 | sendMsg.desire_position = g_scramble_position; 273 | ROS_INFO("have vacancy, send scramble!"); 274 | g_chatter_pub.publish(sendMsg); 275 | g_scramble_count = 0; 276 | } 277 | } 278 | } 279 | } 280 | 281 | /* 282 | * Service function 283 | * respose the self desired position and leader id 284 | * */ 285 | bool requestPosition(formation::PositionRequest::Request &req, 286 | formation::PositionRequest::Response &res) 287 | { 288 | HeartBeatInfo hinfo = *(g_grabbedPositions.begin()); 289 | int leader_id = hinfo.getRobotID(); 290 | if(g_desirePositions.size() > 0 && leader_id != -1) 291 | { 292 | res.position = g_desire_position; 293 | res.leader_id = leader_id; 294 | ROS_INFO("leader_id: %d", leader_id); 295 | ROS_INFO("Service Robot%d Desired Position: %d", g_my_id, g_desire_position); 296 | return true; 297 | } 298 | else 299 | { 300 | return false; 301 | } 302 | } 303 | 304 | int main(int argc, char **argv) 305 | { 306 | ros::init(argc, argv, "none_leader_discussion"); 307 | ros::NodeHandle nh; 308 | std::string ns = nh.getNamespace(); 309 | g_my_id = atoi(ns.substr(7, ns.size() - 7).c_str()); 310 | 311 | g_chatter_pub = nh.advertise("none_leader_topic", 1000); 312 | ros::Subscriber chartter_sub = nh.subscribe("none_leader_topic", 1000, &receiver); 313 | 314 | ros::ServiceServer service = nh.advertiseService("position_requestion", requestPosition); 315 | 316 | ros::Timer timer = nh.createTimer(ros::Duration(0.5), timerCallback); 317 | 318 | ROS_INFO("Sleep 1 sec for initialization!"); 319 | ros::Duration(1).sleep(); 320 | //SCRAMBLE 321 | g_scramble_position = getScramblePosition(); 322 | formation::ChatContent sendMsg; 323 | sendMsg.msg_type = SCRAMBLE; 324 | sendMsg.robot_id = g_my_id; 325 | sendMsg.desire_position = g_scramble_position; 326 | g_chatter_pub.publish(sendMsg); 327 | g_scramble_count = 0; 328 | 329 | ros::Rate loop_rate(10); 330 | while (ros::ok()) 331 | { 332 | if(g_desirePositions.size() > 0) 333 | { 334 | DesirePositionInfo dinfo = *(g_desirePositions.begin()); 335 | g_desire_position = dinfo.getPosition(); 336 | ROS_INFO("Robot%d Desired Position: %d", g_my_id, g_desire_position); 337 | } 338 | ros::spinOnce(); 339 | loop_rate.sleep(); 340 | } 341 | return 0; 342 | } 343 | -------------------------------------------------------------------------------- /src/reference/leader_reference.cpp: -------------------------------------------------------------------------------- 1 | #include "formation/reference/leader_reference.h" 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace formation; 7 | 8 | LeaderRerence::LeaderRerence(tf::TransformListener* tf, Formation pF, double pDS) 9 | { 10 | tfListener_ = tf; 11 | f = pF; 12 | desiredSpaceing = pDS; 13 | mNameSpace_ = nh_.getNamespace(); 14 | } 15 | 16 | bool LeaderRerence::getRobotPose(tf::Stamped& global_pose, int leader_id) const 17 | { 18 | std::string global_frame = "/map"; 19 | std::string leader_base_frame; 20 | std::stringstream leader_id_stream; 21 | leader_id_stream< robot_pose; 25 | robot_pose.setIdentity(); 26 | robot_pose.frame_id_ = leader_base_frame; 27 | robot_pose.stamp_ = ros::Time(); 28 | ros::Time current_time = ros::Time::now(); // save time for checking tf delay later 29 | 30 | // get the global pose of the robot 31 | try 32 | { 33 | // ROS_INFO(" getting robot pose"); 34 | tfListener_->transformPose(global_frame, robot_pose, global_pose); 35 | } 36 | catch (tf::LookupException& ex) 37 | { 38 | ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what()); 39 | return false; 40 | } 41 | catch (tf::ConnectivityException& ex) 42 | { 43 | ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what()); 44 | return false; 45 | } 46 | catch (tf::ExtrapolationException& ex) 47 | { 48 | ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what()); 49 | return false; 50 | } 51 | catch (tf2::InvalidArgumentException& ex) 52 | { 53 | ROS_ERROR_THROTTLE(1.0, "InvalidArgumentException: %s\n", ex.what()); 54 | return false; 55 | } 56 | 57 | double transform_tolerance = 0.1; 58 | // check global_pose timeout 59 | if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance) 60 | { 61 | ROS_WARN_THROTTLE(1.0, 62 | "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f", 63 | current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance); 64 | return false; 65 | } 66 | // ROS_INFO("global_pose.frame_id_: %s", global_pose.frame_id_.c_str()); 67 | return true; 68 | } 69 | 70 | bool LeaderRerence::tramformToOdom(tf::Stamped& orign_pose, geometry_msgs::PoseStamped& output_pose) const 71 | { 72 | std::string output_frame = mNameSpace_.substr(1, mNameSpace_.length() - 1) + "_odom"; 73 | tf::Stamped output_pose_odom; 74 | output_pose_odom.setIdentity(); 75 | ros::Time current_time = ros::Time::now(); // save time for checking tf delay later 76 | 77 | // get the global pose of the robot 78 | try 79 | { 80 | // ROS_INFO(" getting robot pose"); 81 | tfListener_->transformPose(output_frame, orign_pose, output_pose_odom); 82 | } 83 | catch (tf::LookupException& ex) 84 | { 85 | ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what()); 86 | return false; 87 | } 88 | catch (tf::ConnectivityException& ex) 89 | { 90 | ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what()); 91 | return false; 92 | } 93 | catch (tf::ExtrapolationException& ex) 94 | { 95 | ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what()); 96 | return false; 97 | } 98 | catch (tf2::InvalidArgumentException& ex) 99 | { 100 | ROS_ERROR_THROTTLE(1.0, "InvalidArgumentException: %s\n", ex.what()); 101 | return false; 102 | } 103 | 104 | double transform_tolerance = 0.1; 105 | // check global_pose timeout 106 | if (current_time.toSec() - output_pose_odom.stamp_.toSec() > transform_tolerance) 107 | { 108 | ROS_WARN_THROTTLE(1.0, 109 | "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f", 110 | current_time.toSec(), output_pose_odom.stamp_.toSec(), transform_tolerance); 111 | return false; 112 | } 113 | 114 | tf::poseStampedTFToMsg(output_pose_odom, output_pose); 115 | return true; 116 | } 117 | 118 | bool LeaderRerence::transformToMap(geometry_msgs::PoseStamped& globalPlanPose, tf::Stamped& output_pose_map) 119 | { 120 | tf::Stamped orign_pose; 121 | tf::poseStampedMsgToTF(globalPlanPose, orign_pose); 122 | 123 | std::string output_frame = "/map"; 124 | output_pose_map.setIdentity(); // Identity is very importance for using to transform the orientation.Without it, we can not transform correctly. Remenber this!!! 125 | // orign_pose.setIdentity(); // Here, But we can't set orign_pose.setIdentity again! 126 | output_pose_map.stamp_ = ros::Time(); //Reset the time, so it can transform even this stamp is outtime. 127 | orign_pose.stamp_ = ros::Time(); 128 | output_pose_map.frame_id_ = output_frame; 129 | 130 | try 131 | { 132 | tfListener_->transformPose(output_frame, orign_pose, output_pose_map); 133 | } 134 | catch (tf::LookupException& ex) 135 | { 136 | ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what()); 137 | return false; 138 | } 139 | catch (tf::ConnectivityException& ex) 140 | { 141 | ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what()); 142 | return false; 143 | } 144 | catch (tf::ExtrapolationException& ex) 145 | { 146 | ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what()); 147 | return false; 148 | } 149 | catch (tf2::InvalidArgumentException& ex) 150 | { 151 | ROS_ERROR_THROTTLE(1.0, "InvalidArgumentException: %s\n", ex.what()); 152 | return false; 153 | } 154 | // ROS_INFO("output_pose_map x:%f y:%f z:%f, x:%f y:%f z:%f w:%f", output_pose_map.getOrigin().getX(), output_pose_map.getOrigin().getY(), 155 | // output_pose_map.getOrigin().getZ(), output_pose_map.getRotation().getX(), output_pose_map.getRotation().getY(), output_pose_map.getRotation().getZ(), 156 | // output_pose_map.getRotation().getW()); 157 | 158 | return true; 159 | } 160 | 161 | bool LeaderRerence::getDesiredPosition(geometry_msgs::PoseStamped& outputPosition, int position, 162 | int leader_id, geometry_msgs::PoseStamped globalPlanPose) 163 | { 164 | // std::string mNameSpace = mNameSpace_.substr(2, mNameSpace_.length() - 2); 165 | tf::Stamped leaderPos; 166 | 167 | tf::Stamped poseToMap; 168 | transformToMap(globalPlanPose, poseToMap); 169 | double yawOfPath = tf::getYaw(poseToMap.getRotation()); 170 | ROS_INFO("yawOfPath_map: %f", yawOfPath); 171 | 172 | // can't get the pose os leader 173 | if(!getRobotPose(leaderPos, leader_id)) 174 | { 175 | ROS_WARN("Can not get DesiredPosition!"); 176 | return false; 177 | } 178 | ROS_INFO("leaderPos x:%f y:%f z:%f, x:%f y:%f z:%f w:%f", leaderPos.getOrigin().getX(), leaderPos.getOrigin().getY(), 179 | leaderPos.getOrigin().getZ(), leaderPos.getRotation().getX(), leaderPos.getRotation().getY(), leaderPos.getRotation().getZ(), 180 | leaderPos.getRotation().getW()); 181 | 182 | tf::Stamped desiredPosition(leaderPos); 183 | desiredPosition.setRotation(poseToMap.getRotation()); 184 | 185 | desiredPosition.getOrigin().setY(leaderPos.getOrigin().getY() + position * desiredSpaceing * sin(yawOfPath - M_PI_2)); 186 | desiredPosition.getOrigin().setX(leaderPos.getOrigin().getX() + position * desiredSpaceing * cos(yawOfPath - M_PI_2)); 187 | 188 | ROS_INFO("desiredPosition x:%f y:%f z:%f, x:%f y:%f z:%f w:%f", desiredPosition.getOrigin().getX(), desiredPosition.getOrigin().getY(), 189 | desiredPosition.getOrigin().getZ(), desiredPosition.getRotation().getX(), desiredPosition.getRotation().getY(), desiredPosition.getRotation().getZ(), 190 | desiredPosition.getRotation().getW()); 191 | tramformToOdom(desiredPosition, outputPosition); 192 | return true; 193 | } 194 | 195 | -------------------------------------------------------------------------------- /srv/PositionRequest.srv: -------------------------------------------------------------------------------- 1 | --- 2 | int64 position 3 | int64 leader_id 4 | --------------------------------------------------------------------------------