├── .catkin_workspace ├── .gitattributes ├── LICENSE ├── README.md └── src ├── crazyswarm_ros ├── CMakeLists.txt ├── config │ ├── config_am_swarm.yaml │ ├── crazyflies.yaml │ └── crazyflies_21.yaml ├── launch │ └── start_crazyswarm_sim.launch ├── msg │ ├── FullState.msg │ ├── GenericLogData.msg │ ├── Person.msg │ ├── Position.msg │ ├── TrajectoryPolynomialPiece.msg │ └── VelocityWorld.msg ├── package.xml ├── scripts │ ├── __pycache__ │ │ └── uav_trajectory.cpython-37.pyc │ ├── crazyswarm_sim.py │ └── pycrazyswarm │ │ ├── __init__.py │ │ ├── __pycache__ │ │ └── __init__.cpython-310.pyc │ │ ├── cfsim │ │ ├── .gitignore │ │ ├── Makefile │ │ ├── __init__.py │ │ ├── __pycache__ │ │ │ ├── __init__.cpython-310.pyc │ │ │ └── cffirmware.cpython-310.pyc │ │ ├── cffirmware.i │ │ ├── numpy.i │ │ └── setup.py │ │ ├── crazyflie.cpython-310-x86_64-linux-gnu.so │ │ ├── crazyflieSim.cpython-310-x86_64-linux-gnu.so │ │ ├── crazyflie_forward_model.cpython-310-x86_64-linux-gnu.so │ │ ├── crazyswarm_py.cpython-310-x86_64-linux-gnu.so │ │ ├── msg │ │ ├── _FullState.cpython-310-x86_64-linux-gnu.so │ │ ├── _GenericLogData.cpython-310-x86_64-linux-gnu.so │ │ ├── _Position.cpython-310-x86_64-linux-gnu.so │ │ ├── _TrajectoryPolynomialPiece.cpython-310-x86_64-linux-gnu.so │ │ ├── _VelocityWorld.cpython-310-x86_64-linux-gnu.so │ │ ├── __init__.py │ │ └── __pycache__ │ │ │ └── __init__.cpython-310.pyc │ │ ├── srv │ │ ├── _AddInts.py │ │ ├── _GoTo.cpython-310-x86_64-linux-gnu.so │ │ ├── _Land.cpython-310-x86_64-linux-gnu.so │ │ ├── _StartTrajectory.cpython-310-x86_64-linux-gnu.so │ │ ├── _Takeoff.cpython-310-x86_64-linux-gnu.so │ │ ├── _UploadTrajectory.cpython-310-x86_64-linux-gnu.so │ │ ├── __init__.py │ │ └── __pycache__ │ │ │ ├── _AddInts.cpython-310.pyc │ │ │ └── __init__.cpython-310.pyc │ │ ├── util.cpython-310-x86_64-linux-gnu.so │ │ └── visualizer │ │ ├── __init__.py │ │ ├── __pycache__ │ │ └── __init__.cpython-310.pyc │ │ ├── crazyflie2.obj.gz │ │ ├── visMatplotlib.cpython-310-x86_64-linux-gnu.so │ │ ├── visNull.cpython-310-x86_64-linux-gnu.so │ │ └── visVispy.cpython-310-x86_64-linux-gnu.so └── srv │ ├── AddInts.srv │ ├── GoTo.srv │ ├── Land.srv │ ├── StartTrajectory.srv │ ├── Takeoff.srv │ └── UploadTrajectory.srv ├── imgs ├── crazyswarm_demo01.gif ├── crazyswarm_demo02.gif ├── logo.jpg └── wheelswarm_demo01.gif ├── ros_env_py310.yaml ├── swarm_brain ├── CMakeLists.txt ├── launch │ ├── start_crazyswarm_crazyflie_gcs.launch │ ├── start_crazyswarm_go_circle.launch │ └── start_wheelswarm_swarm_brain.launch ├── package.xml └── scripts │ ├── crazyswarm │ ├── __pycache__ │ │ └── uav_trajectory.cpython-310.pyc │ ├── crazyflie_gcs.py │ ├── figure8.csv │ ├── go_circle.py │ ├── pycrazyswarm │ │ ├── __init__.py │ │ ├── __pycache__ │ │ │ └── __init__.cpython-310.pyc │ │ ├── msg │ │ │ ├── _FullState.py │ │ │ ├── _GenericLogData.py │ │ │ ├── _Position.py │ │ │ ├── _TrajectoryPolynomialPiece.py │ │ │ ├── _VelocityWorld.py │ │ │ ├── __init__.py │ │ │ ├── __pycache__ │ │ │ │ ├── _FullState.cpython-310.pyc │ │ │ │ ├── _GenericLogData.cpython-310.pyc │ │ │ │ ├── _Position.cpython-310.pyc │ │ │ │ ├── _TrajectoryPolynomialPiece.cpython-310.pyc │ │ │ │ ├── _VelocityWorld.cpython-310.pyc │ │ │ │ └── __init__.cpython-310.pyc │ │ │ └── setup.py │ │ └── srv │ │ │ ├── _AddInts.py │ │ │ ├── _GoTo.py │ │ │ ├── _Land.py │ │ │ ├── _StartTrajectory.py │ │ │ ├── _Takeoff.py │ │ │ ├── _UploadTrajectory.py │ │ │ ├── __init__.py │ │ │ ├── __pycache__ │ │ │ ├── _AddInts.cpython-310.pyc │ │ │ ├── _GoTo.cpython-310.pyc │ │ │ ├── _Land.cpython-310.pyc │ │ │ ├── _StartTrajectory.cpython-310.pyc │ │ │ ├── _Takeoff.cpython-310.pyc │ │ │ ├── _UploadTrajectory.cpython-310.pyc │ │ │ └── __init__.cpython-310.pyc │ │ │ └── setup.py │ └── uav_trajectory.py │ └── wheelswarm │ ├── swarm_brain.py │ └── utilities │ ├── __init__.py │ ├── __pycache__ │ └── __init__.cpython-310.pyc │ ├── barrier_certificates.cpython-310-x86_64-linux-gnu.so │ ├── controllers.cpython-310-x86_64-linux-gnu.so │ ├── graph.cpython-310-x86_64-linux-gnu.so │ ├── misc.cpython-310-x86_64-linux-gnu.so │ └── transformations.cpython-310-x86_64-linux-gnu.so ├── swarm_view ├── CMakeLists.txt ├── launch │ └── start_pybullet4crazyswarm.launch ├── package.xml └── scripts │ ├── assets │ ├── architrave.urdf │ ├── cf2.dae │ ├── cf2p.urdf │ └── cf2x.urdf │ ├── pybullet_crazyswarm.py │ └── pycrazyswarm │ ├── __init__.py │ ├── __pycache__ │ └── __init__.cpython-310.pyc │ └── msg │ ├── _FullState.cpython-310-x86_64-linux-gnu.so │ ├── _GenericLogData.cpython-310-x86_64-linux-gnu.so │ ├── _Position.cpython-310-x86_64-linux-gnu.so │ ├── _TrajectoryPolynomialPiece.cpython-310-x86_64-linux-gnu.so │ ├── _VelocityWorld.cpython-310-x86_64-linux-gnu.so │ ├── __init__.py │ └── __pycache__ │ └── __init__.cpython-310.pyc └── wheelswarm_ros ├── CMakeLists.txt ├── config └── wheelswarm.yaml ├── launch └── start_wheelswarm.launch ├── package.xml └── scripts └── rps ├── __init__.py ├── __version__.py ├── kkrobot.cpython-310-x86_64-linux-gnu.so ├── kkrobot_abc.cpython-310-x86_64-linux-gnu.so ├── setup.py ├── utilities ├── __init__.py ├── __pycache__ │ └── __init__.cpython-310.pyc └── misc.cpython-310-x86_64-linux-gnu.so └── wheelswarm_sim.py /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, AISwarmLab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AISwarmLab 2 | 3 | 4 | 5 | 智能蜂群实验室（AI SwarmLab®）依托华东师范大学信息学部，汇集了 一支充满激情和创新的团队，致力于协同控制、强化学习、智能博弈、大模型等先进技术研究，专注于为研究人员和工程师提供跨域无人系统大规模集群虚实结合的轻量级可信仿真平台，为集群协同和博弈对抗算法提供集群数量规模可拓、场景复杂度可变、集群动态拓扑可调的综合解决方案。 6 | 7 | 8 | 9 | ## 环境安装 10 | 11 | 操作系统要求：Ubuntu20.04 12 | 13 | ROS：Noetic 14 | 15 | Python：Python3.10 16 | 17 | Mamba：Mambaforge-pypy3-Linux-x86_64 18 | 19 | ## 1. 安装Mamba 20 | 21 | 下载Mamba安装包 22 | 23 | 在终端输入`bash Mambaforge-pypy3-Linux-x86_64.sh`，并按照提示缺省安装。 24 | 25 | ## 2.安装Python和ROS 26 | 27 | 将swarm_simulator_ws源代码下载到本地，启动终端，进入源码目录，如 28 | 29 | `cd ~/swarm_simulator_ws/src` 30 | 31 | 在终端输入`source ~/mambaforge-pypy3/bin/activate`，激活base环境。 32 | 33 | 激活base环境后，在终端输入`mamba env create -n ros_env_py310 --file ros_env_py310.yaml` 34 | 35 | 安装完成依赖环境后，关闭终端。 36 | 另外启动一个终端，激活ros_env_py310 环境，依次输入 37 | 38 | `source ~/mambaforge-pypy3/bin/activate/ros_env_py310` 39 | 40 | 激活ros_env_py310 环境后，输入roscore。若有报错，请删除home文件夹下的.ros文件夹，重启，再次激活ros_env_py310 环境，输入roscore。 41 | 42 | ## 3.编译源代码 43 | 44 | 将swarm_simulator_ws源代码下载到本地，启动终端，激活ros_env_py310 环境，依次输入 45 | 46 | `cd ~/swarm_simulator_ws/` 47 | 48 | `catkin_make` 49 | 50 | 等待编译完成。 51 | 52 | ## 基本用法 53 | ## 1.WheelswarmSimulator 54 | 订阅无人车的状态消息，话题名称（如1号车）为"/robot_1/pose"，消息类型为Odometry，msg.pose.pose为无人车的位姿信息； 55 | 56 | 发布无人车的控制消息，话题名称（如1号车）为"/robot_1/cmd_vel"，消息类型为Twist，msg.linear.x为无人车的控制线速度，msg.angular.z为无人车的控制角速度 57 | 58 | 示例1 59 | 60 | 61 | 62 |
图1 无人车集群互换位置避碰示例
63 | 64 | 如图1所示，在半径为1m和2m的圆内分别随机放置10辆无人车，外环的10辆无人车彼此交换位置，内环的10辆无人车彼此交换位置，20辆无人车在交换位置过程中要考虑避碰情况。 65 | 66 | **终端1（启动WheelswarmSimulator）：** 67 | 68 | `source ~/mambaforge-pypy3/bin/activate/ros_env_py310` 69 | 70 | `source ~/swarm_simulator_ws/devel/setup.bash` 71 | 72 | `roslaunch wheelswarm_ros start_wheelswarm.launch` 73 | 74 | **终端2（启动无人车集群控制示例程序）：** 75 | 76 | `source ~/mambaforge-pypy3/bin/activate/ros_env_py310` 77 | 78 | `source ~/swarm_simulator_ws/devel/setup.bash` 79 | 80 | `roslaunch swarm_brain start_wheelswarm_swarm_brain.launch` 81 | 82 | ## 2.CrazyswarmSimulator 83 | **高级飞行控制指令** 84 | **起飞指令** 85 | 86 | TakeoffRequest(id, groupMask, height, duration)，其中，id为无人机编号，groupMask为集群编号，缺省为0，height为无人机起飞高度(m)，duration为持续时间(s)； 87 | 88 | **降落指令** 89 | 90 | LandRequest(id, groupMask, height, duration)，其中，id为无人机编号，groupMask为集群编号，缺省为0，height为无人机降落高度(m)，duration为持续时间(s)； 91 | 92 | **GoTo指令** 93 | 94 | GoToRequest(id, groupMask, relative, goal, yaw, duration)，其中，id为无人机编号，groupMask为集群编号，缺省为0，relative是否为相对位置，缺省为False, goal为目标点，yaw为偏航角(rad)，duration为持续时间(s)； 95 | 96 | **上传轨迹指令** 97 | 98 | UploadTrajectoryRequest(id, trajectoryId, pieceOffset, pieces)，id为无人机编号，trajectoryId为上传轨迹id，pieceOffset为轨迹片段偏移量，pieces为轨迹片段； 99 | 100 | **沿轨迹飞行指令** 101 | 102 | StartTrajectoryRequest(id, groupMask, trajectoryId, timescale, reversed, relative)，id为无人机编号，groupMask为集群编号，缺省为0，timescale为时间缩放比例，缺省为1.0，reversed是否按照相反的轨迹顺序飞行，缺省为False，relative是否为相对位置，缺省为True； 103 | 104 | **低级飞行控制指令** 105 | 106 | **cmd_vel** 107 | 108 | 话题名称（如1号机）为"/cf1/cmd_vel"，消息类型为Twist，msg.twist.linear为无人机的线速度信息，msg.twist.angular为无人机的角速度信息； 109 | 110 | **cmd_velocity_world** 111 | 112 | 话题名称（如1号机）为"/cf1/cmd_velocity_world"，消息类型为VelocityWorld，msg.vel为无人车的控制速度矢量（x,y,z），msg.yawRate为无人机的偏航角速度，缺省为0； 113 | 114 | **cmd_postion** 115 | 116 | 话题名称（如1号机）为"/cf1/cmd_position"，消息类型为Position，msg.x、msg.y、msg.z为无人机的位置，msg.yaw为无人机的偏航角，缺省为0； 117 | 118 | **cmd_full_state** 119 | 120 | 话题名称（如1号机）为"/cf1/cmd_full_state"，消息类型为FullState，msg.pose.position为无人机的位置，msg.pose.orientation为无人机的控制姿态（四元数），msg.twist.linear为无人机的控制线速度，msg.acc为无人机的控制加速度，msg.twist.angular为无人机的控制角速度。 121 | 122 | ### 示例1 123 | 124 | 如图2所示，在终端输入take_off、land、upload、fly、goto，分别实现起飞、降落、上传轨迹、按飞机飞行和GoTo等无人机高级控制指令。 125 | 126 | 127 | 128 |
图2 无人机集群高级飞行控制示例
129 | 130 | **终端1（启动CrazyswarmSimulator）：** 131 | 132 | `source ~/mambaforge-pypy3/bin/activate/ros_env_py310` 133 | 134 | `source ~/swarm_simulator_ws/devel/setup.bash` 135 | 136 | `roslaunch crazyswarm_ros start_crazyswarm_sim.launch` 137 | 138 | **终端2（启动Pybullet视景窗口）：** 139 | 140 | `source ~/mambaforge-pypy3/bin/activate/ros_env_py310` 141 | 142 | `source ~/swarm_simulator_ws/devel/setup.bash` 143 | 144 | `roslaunch swarm_view start_pybullet4crazyswarm.launch` 145 | 146 | **终端3（启动无人机集群高级飞行控制示例程序）：** 147 | 148 | `source ~/mambaforge-pypy3/bin/activate/ros_env_py310` 149 | 150 | `source ~/swarm_simulator_ws/devel/setup.bash` 151 | 152 | `roslaunch swarm_brain start_crazyswarm_crazyflie_gcs.launch` 153 | 154 | ### 示例2 155 | 156 | 如图3所示，10架无人机起飞后，绕着原点转圈飞行。 157 | 158 | 159 | 160 |
图3 无人机集群转圈飞行示例
161 | 162 | **终端1（启动CrazyswarmSimulator）：** 163 | 164 | `source ~/mambaforge-pypy3/bin/activate/ros_env_py310` 165 | 166 | `source ~/swarm_simulator_ws/devel/setup.bash` 167 | 168 | `roslaunch crazyswarm_ros start_crazyswarm_sim.launch` 169 | 170 | **终端2（启动Pybullet视景窗口）：** 171 | 172 | `source ~/mambaforge-pypy3/bin/activate/ros_env_py310` 173 | 174 | `source ~/swarm_simulator_ws/devel/setup.bash` 175 | 176 | `roslaunch swarm_view start_pybullet4crazyswarm.launch` 177 | 178 | **终端3（启动无人机集群转圈飞行控制示例程序）：** 179 | 180 | `source ~/mambaforge-pypy3/bin/activate/ros_env_py310` 181 | 182 | `source ~/swarm_simulator_ws/devel/setup.bash` 183 | 184 | `roslaunch swarm_brain start_crazyswarm_go_circle.launch` 185 | 186 | ## `联系我们` 187 | 188 | 在 SwarmLab®，我们不仅提供先进的解决方案，还提供专业的技术支持和定制化服务，确保您能够充分发挥产品的潜力，实现研究目标。我们的解决方案涵盖协同控制、强化学习、智能博弈、大模型等领域，并为您的研究提供全方位的支持。 189 | 190 | 选择 SwarmLab®的产品，意味着选择了高品质、高性能和高效率。让我们携手合作，共同探索蜂群技术的未来，助力您的研究取得更大的成功！欢迎联系我们，了解更多关于 SwarmLab®信息，让我们一起开启创新之旅！ 191 | 192 | ​ 欢迎访问我们： 193 | 194 | ​ https://swarmlab.net/ 195 | 196 | ​ https://swarmlab.cn/ 197 | 198 | ​ https://exbot.net/ 199 | 200 | ​ 微信视频号：SwarmLab 201 | 202 | ​ 联系方式：15821292799 田先生 203 | 204 | #### _Enjoy!_ :+1: 205 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(crazyswarm_ros) 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 | geometry_msgs 15 | message_generation 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 | Position.msg 55 | TrajectoryPolynomialPiece.msg 56 | VelocityWorld.msg 57 | FullState.msg 58 | GenericLogData.msg 59 | ) 60 | 61 | ## Generate services in the 'srv' folder 62 | add_service_files( 63 | FILES 64 | Takeoff.srv 65 | Land.srv 66 | GoTo.srv 67 | UploadTrajectory.srv 68 | ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | generate_messages( 79 | DEPENDENCIES 80 | std_msgs 81 | geometry_msgs 82 | ) 83 | 84 | ################################################ 85 | ## Declare ROS dynamic reconfigure parameters ## 86 | ################################################ 87 | 88 | ## To declare and build dynamic reconfigure parameters within this 89 | ## package, follow these steps: 90 | ## * In the file package.xml: 91 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 92 | ## * In this file (CMakeLists.txt): 93 | ## * add "dynamic_reconfigure" to 94 | ## find_package(catkin REQUIRED COMPONENTS ...) 95 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 96 | ## and list every .cfg file to be processed 97 | 98 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 99 | # generate_dynamic_reconfigure_options( 100 | # cfg/DynReconf1.cfg 101 | # cfg/DynReconf2.cfg 102 | # ) 103 | 104 | ################################### 105 | ## catkin specific configuration ## 106 | ################################### 107 | ## The catkin_package macro generates cmake config files for your package 108 | ## Declare things to be passed to dependent projects 109 | ## INCLUDE_DIRS: uncomment this if your package contains header files 110 | ## LIBRARIES: libraries you create in this project that dependent projects also need 111 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 112 | ## DEPENDS: system dependencies of this project that dependent projects also need 113 | catkin_package( 114 | # INCLUDE_DIRS include 115 | # LIBRARIES crazyswarm_ros 116 | CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs message_runtime 117 | # DEPENDS system_lib 118 | ) 119 | 120 | ########### 121 | ## Build ## 122 | ########### 123 | 124 | ## Specify additional locations of header files 125 | ## Your package locations should be listed before other locations 126 | include_directories( 127 | # include 128 | ${catkin_INCLUDE_DIRS} 129 | ) 130 | 131 | ## Declare a C++ library 132 | # add_library(${PROJECT_NAME} 133 | # src/${PROJECT_NAME}/crazyswarm_ros.cpp 134 | # ) 135 | 136 | ## Add cmake target dependencies of the library 137 | ## as an example, code may need to be generated before libraries 138 | ## either from message generation or dynamic reconfigure 139 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | ## Declare a C++ executable 142 | ## With catkin_make all packages are built within a single CMake context 143 | ## The recommended prefix ensures that target names across packages don't collide 144 | # add_executable(${PROJECT_NAME}_node src/crazyswarm_ros_node.cpp) 145 | 146 | ## Rename C++ executable without prefix 147 | ## The above recommended prefix causes long target names, the following renames the 148 | ## target back to the shorter version for ease of user use 149 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 150 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 151 | 152 | ## Add cmake target dependencies of the executable 153 | ## same as for the library above 154 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 155 | 156 | ## Specify libraries to link a library or executable target against 157 | # target_link_libraries(${PROJECT_NAME}_node 158 | # ${catkin_LIBRARIES} 159 | # ) 160 | 161 | ############# 162 | ## Install ## 163 | ############# 164 | 165 | # all install targets should use catkin DESTINATION variables 166 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 167 | 168 | ## Mark executable scripts (Python etc.) for installation 169 | ## in contrast to setup.py, you can choose the destination 170 | catkin_install_python(PROGRAMS 171 | scripts/crazyswarm_sim.py 172 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | ) 174 | 175 | ## Mark executables for installation 176 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 177 | # install(TARGETS ${PROJECT_NAME}_node 178 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark libraries for installation 182 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 183 | # install(TARGETS ${PROJECT_NAME} 184 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 185 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 186 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 187 | # ) 188 | 189 | ## Mark cpp header files for installation 190 | # install(DIRECTORY include/${PROJECT_NAME}/ 191 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 192 | # FILES_MATCHING PATTERN "*.h" 193 | # PATTERN ".svn" EXCLUDE 194 | # ) 195 | 196 | ## Mark other files for installation (e.g. launch and bag files, etc.) 197 | # install(FILES 198 | # # myfile1 199 | # # myfile2 200 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 201 | # ) 202 | 203 | ############# 204 | ## Testing ## 205 | ############# 206 | 207 | ## Add gtest based cpp test target and link libraries 208 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_crazyswarm_ros.cpp) 209 | # if(TARGET ${PROJECT_NAME}-test) 210 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 211 | # endif() 212 | 213 | ## Add folders to be run by python nosetests 214 | # catkin_add_nosetests(test) 215 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/config/config_am_swarm.yaml: -------------------------------------------------------------------------------- 1 | # @@ Debugging Options 2 | verbose: 0 # Residual print = 1, DistanceToGoal check = 2, Collision print = 3, Compute time print = 4, Scaled Thurst print = 5 3 | 4 | num_drone: 10 # ignored if read_config is false 5 | num_obs: 16 # ignored if read_config is false 6 | 7 | # @@ For axis_wise set true otherwise quadratic 8 | axis_wise: false 9 | jerk_snap_constraints: false 10 | 11 | # @@ CBF 12 | gamma: 1.0 13 | 14 | # @@ Planning Parameters 15 | world: 3 16 | num: 30 17 | t_plan: 3 18 | num_up: 300 19 | dist_stop: 0.2 20 | free_space: false 21 | kappa: 10 22 | 23 | 24 | # @@ Optimizer Parameters 25 | max_iter: 200 26 | max_time: 60 27 | thresold: 0.005 28 | 29 | order_smoothness: 2 30 | weight_goal: 5000 31 | weight_smoothness: 150 32 | delta_aggressive: 1.0 33 | 34 | delta_static_obs: 1.20 35 | delta_drone: 1.15 36 | delta_vel: 1.33 37 | delta_acc: 1.30 38 | delta_jerk: 1.30 39 | delta_snap: 1.30 40 | delta_ineq: 1.30 41 | 42 | rho_static_obs_max: 200000 43 | rho_drone_max: 200000 44 | rho_vel_max: 200000 45 | rho_acc_max: 200000 46 | rho_jerk_max: 200000 47 | rho_snap_max: 200000 48 | rho_ineq_max: 200000 49 | 50 | 51 | # true kinematic values 52 | vel_max: 1.5 53 | jerk_max: 8.0 54 | snap_max: 8.0 55 | 56 | # scaled thrust values 57 | use_thrust_values: true 58 | gravity: 9.8 59 | f_max: 1.5 # 1.5g 60 | f_min: 0.3 # 0.3g 61 | 62 | acc_max: 2.8 63 | 64 | a_drone: 0.10 65 | b_drone: 0.10 66 | c_drone: 0.20 67 | 68 | prox_agent: 0.2 69 | prox_obs: 0.2 70 | buffer: 0.05 71 | 72 | 73 | # @@ Room limits 74 | x_lim: [-2.0, 2.0] 75 | y_lim: [-2.0, 2.0] 76 | z_lim: [+0.1, 2.2] 77 | 78 | 79 | 80 | # @@ Obstacle Configuration 81 | 82 | pos_static_obs: [[-0.441, -1.518, 0], 83 | [-1.359, 1.535, 0], 84 | [0.424, 1.006, 0], 85 | [0.029, -0.829, 0], 86 | [-1.562, -1.624, 0], 87 | [-1.135, -1.179, 0], 88 | [1.139, -0.309, 0], 89 | [-0.046, 1.452, 0], 90 | [-0.003, -0.128, 0], 91 | [-1.146, 0.37, 0], 92 | [-0.708, 1.038, 0], 93 | [0.264, -1.56, 0], 94 | [0.744, 0.37, 0], 95 | [0.84, -1.03, 0], 96 | [-0.644, -0.701, 0], 97 | [1.31, -1.497, 0]] 98 | 99 | dim_static_obs: [[+0.13, +0.13, 10.0], 100 | [+0.13, +0.13, 10.0], 101 | [+0.13, +0.13, 10.0], 102 | [+0.13, +0.13, 10.0], 103 | [+0.13, +0.13, 10.0], 104 | [+0.13, +0.13, 10.0], 105 | [+0.13, +0.13, 10.0], 106 | [+0.13, +0.13, 10.0], 107 | [+0.13, +0.13, 10.0], 108 | [+0.13, +0.13, 10.0], 109 | [+0.13, +0.13, 10.0], 110 | [+0.13, +0.13, 10.0], 111 | [+0.13, +0.13, 10.0], 112 | [+0.13, +0.13, 10.0], 113 | [+0.13, +0.13, 10.0], 114 | [+0.13, +0.13, 10.0]] 115 | 116 | 117 | init_drone: [[1.759, 0.773, 0.5], 118 | [-0.142, -1.836, 0.5], 119 | [0.307, 1.834, 0.5], 120 | [-1.765, -0.404, 0.5], 121 | [1.748, 1.77, 0.5], 122 | [1.727, 0.434, 0.5], 123 | [-1.071, -0.097, 0.5], 124 | [0.68, -1.826, 0.5], 125 | [-1.819, 0.55, 0.5], 126 | [-1.787, -0.065, 0.5]] 127 | 128 | goal_drone: [[-1.787, -0.065, 0.5], 129 | [-1.765, -0.404, 0.5], 130 | [1.759, 0.773, 0.5], 131 | [-0.142, -1.836, 0.5], 132 | [1.727, 0.434, 0.5], 133 | [1.748, 1.77, 0.5], 134 | [0.68, -1.826, 0.5], 135 | [0.307, 1.834, 0.5], 136 | [-1.819, 0.55, 0.5], 137 | [-1.071, -0.097, 0.5]] -------------------------------------------------------------------------------- /src/crazyswarm_ros/config/crazyflies.yaml: -------------------------------------------------------------------------------- 1 | crazyflies: 2 | - id: 1 3 | channel: 80 4 | initialPosition: [-1.0, 1.0, 0.0] 5 | type: default 6 | - id: 2 7 | channel: 80 8 | initialPosition: [0.0, 1.0, 0.0] 9 | type: default 10 | - id: 3 11 | channel: 80 12 | initialPosition: [1.0, 1.0, 0.0] 13 | type: default 14 | - id: 4 15 | channel: 80 16 | initialPosition: [-1.0, 0.0, 0.0] 17 | type: default 18 | - id: 5 19 | channel: 80 20 | initialPosition: [0.0, 0.0, 0.0] 21 | type: default 22 | - id: 6 23 | channel: 80 24 | initialPosition: [1.0, 0.0, 0.0] 25 | type: default 26 | - id: 7 27 | channel: 80 28 | initialPosition: [-1.0, -1.0, 0.0] 29 | type: default 30 | - id: 8 31 | channel: 80 32 | initialPosition: [0.0, -1.0, 0.0] 33 | type: default 34 | - id: 9 35 | channel: 80 36 | initialPosition: [1.0, -1.0, 0.0] 37 | type: default 38 | - id: 10 39 | channel: 80 40 | initialPosition: [2.0, 1.0, 0.0] 41 | type: default -------------------------------------------------------------------------------- /src/crazyswarm_ros/config/crazyflies_21.yaml: -------------------------------------------------------------------------------- 1 | crazyflies: 2 | - id: 1 3 | channel: 80 4 | initialPosition: [-1.0, 1.0, 0.0] 5 | type: default 6 | - id: 2 7 | channel: 80 8 | initialPosition: [0.0, 1.0, 0.0] 9 | type: default 10 | - id: 3 11 | channel: 80 12 | initialPosition: [1.0, 1.0, 0.0] 13 | type: default 14 | - id: 4 15 | channel: 80 16 | initialPosition: [-1.0, 0.0, 0.0] 17 | type: default 18 | - id: 5 19 | channel: 80 20 | initialPosition: [0.0, 0.0, 0.0] 21 | type: default 22 | - id: 6 23 | channel: 80 24 | initialPosition: [1.0, 0.0, 0.0] 25 | type: default 26 | - id: 7 27 | channel: 80 28 | initialPosition: [-1.0, -1.0, 0.0] 29 | type: default 30 | - id: 8 31 | channel: 80 32 | initialPosition: [0.0, -1.0, 0.0] 33 | type: default 34 | - id: 9 35 | channel: 80 36 | initialPosition: [1.0, -1.0, 0.0] 37 | type: default 38 | - id: 10 39 | channel: 80 40 | initialPosition: [2.0, 1.0, 0.0] 41 | type: default 42 | - id: 11 43 | channel: 80 44 | initialPosition: [3.0, 1.0, 0.0] 45 | type: default 46 | - id: 12 47 | channel: 80 48 | initialPosition: [2.0, 0.0, 0.0] 49 | type: default 50 | - id: 13 51 | channel: 80 52 | initialPosition: [3.0, 0.0, 0.0] 53 | type: default 54 | - id: 14 55 | channel: 80 56 | initialPosition: [2.0, -1.0, 0.0] 57 | type: default 58 | - id: 15 59 | channel: 80 60 | initialPosition: [3.0, -1.0, 0.0] 61 | type: default 62 | - id: 16 63 | channel: 80 64 | initialPosition: [-2.0, 1.0, 0.0] 65 | type: default 66 | - id: 17 67 | channel: 80 68 | initialPosition: [-3.0, 1.0, 0.0] 69 | type: default 70 | - id: 18 71 | channel: 80 72 | initialPosition: [-2.0, 0.0, 0.0] 73 | type: default 74 | - id: 19 75 | channel: 80 76 | initialPosition: [-3.0, 0.0, 0.0] 77 | type: default 78 | - id: 20 79 | channel: 80 80 | initialPosition: [-3.0, -1.0, 0.0] 81 | type: default 82 | - id: 21 83 | channel: 80 84 | initialPosition: [-2.0, -1.0, 0.0] 85 | type: default -------------------------------------------------------------------------------- /src/crazyswarm_ros/launch/start_crazyswarm_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/msg/FullState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Twist twist 4 | geometry_msgs/Vector3 acc 5 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/msg/GenericLogData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] values 3 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/msg/Person.msg: -------------------------------------------------------------------------------- 1 | string name 2 | int32 age 3 | float32 height -------------------------------------------------------------------------------- /src/crazyswarm_ros/msg/Position.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 x 3 | float32 y 4 | float32 z 5 | float32 yaw 6 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/msg/TrajectoryPolynomialPiece.msg: -------------------------------------------------------------------------------- 1 | # 2 | 3 | float32[] px 4 | float32[] py 5 | float32[] pz 6 | float32[] pyaw 7 | float32 duration -------------------------------------------------------------------------------- /src/crazyswarm_ros/msg/VelocityWorld.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3 vel 3 | float32 yawRate -------------------------------------------------------------------------------- /src/crazyswarm_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | crazyswarm_ros 4 | 0.0.0 5 | The crazyswarm_ros package 6 | 7 | 8 | 9 | 10 | amov 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | geometry_msgs 56 | message_generation 57 | roscpp 58 | rospy 59 | std_msgs 60 | roscpp 61 | rospy 62 | std_msgs 63 | geometry_msgs 64 | message_runtime 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/__pycache__/uav_trajectory.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/__pycache__/uav_trajectory.cpython-37.pyc -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/crazyswarm_sim.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from os.path import abspath, join, dirname 3 | sys.path.insert(0, join(abspath(dirname(__file__)))) 4 | 5 | from pycrazyswarm import Crazyswarm 6 | import rospy 7 | import math 8 | from pycrazyswarm.srv import * 9 | 10 | 11 | 12 | 13 | 14 | def main(): 15 | rospy.init_node('crazyswarm_sim', anonymous=True) 16 | 17 | swarm = Crazyswarm() 18 | timeHelper = swarm.timeHelper 19 | 20 | while not rospy.is_shutdown(): 21 | timeHelper.step(timeHelper.dt) 22 | 23 | timeHelper.rate.sleep() 24 | 25 | if __name__ == "__main__": 26 | main() 27 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/__init__.py: -------------------------------------------------------------------------------- 1 | from .crazyswarm_py import * 2 | 3 | __all__ = ["Crazyswarm", "cfsim", "msg"] 4 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/.gitignore: -------------------------------------------------------------------------------- 1 | cffirmware.py 2 | cffirmware_wrap.c 3 | *.so 4 | build 5 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/Makefile: -------------------------------------------------------------------------------- 1 | firmdir = ../../../../../../crazyflie-firmware 2 | modinc = $(firmdir)/src/modules/interface 3 | modsrc = $(firmdir)/src/modules/src 4 | 5 | swig: setup.py cffirmware_wrap.c $(modsrc)/*.c 6 | ifeq ($(CSW_PYTHON),) 7 | $(error Environment variable CSW_PYTHON must be set to "python2" or "python3") 8 | endif 9 | $(CSW_PYTHON) setup.py build_ext --inplace 10 | 11 | cffirmware_wrap.c: cffirmware.i $(modinc)/*.h 12 | swig -python -I$(modinc) cffirmware.i 13 | 14 | clean: 15 | rm -f cffirmware.py _cffirmware.so *.pyc cffirmware_wrap.c 16 | rm -rf build 17 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/__init__.py -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/__pycache__/cffirmware.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/__pycache__/cffirmware.cpython-310.pyc -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/cffirmware.i: -------------------------------------------------------------------------------- 1 | %module cffirmware 2 | 3 | // ignore GNU specific compiler attributes 4 | #define __attribute__(x) 5 | 6 | %{ 7 | #define SWIG_FILE_WITH_INIT 8 | #include "collision_avoidance.h" 9 | #include "math3d.h" 10 | #include "pptraj.h" 11 | #include "planner.h" 12 | #include "stabilizer_types.h" 13 | %} 14 | 15 | %include "collision_avoidance.h" 16 | %include "math3d.h" 17 | %include "pptraj.h" 18 | %include "planner.h" 19 | %include "stabilizer_types.h" 20 | 21 | %include "numpy.i" 22 | 23 | 24 | %init %{ 25 | import_array() 26 | %} 27 | 28 | %apply (int DIM1, float* IN_ARRAY1) {(int nOthers, float const *otherPositions)} 29 | 30 | %inline %{ 31 | void poly4d_set(struct poly4d *poly, int dim, int coef, float val) 32 | { 33 | poly->p[dim][coef] = val; 34 | } 35 | float poly4d_get(struct poly4d *poly, int dim, int coef) 36 | { 37 | return poly->p[dim][coef]; 38 | } 39 | struct poly4d* pp_get_piece(struct piecewise_traj *pp, int i) 40 | { 41 | return &pp->pieces[i]; 42 | } 43 | struct poly4d* malloc_poly4d(int size) 44 | { 45 | return (struct poly4d*)malloc(sizeof(struct poly4d) * size); 46 | } 47 | 48 | struct vec vec2svec(struct vec3_s v) 49 | { 50 | return mkvec(v.x, v.y, v.z); 51 | } 52 | 53 | struct vec3_s svec2vec(struct vec v) 54 | { 55 | struct vec3_s vv = { .x = v.x, .y = v.y, .z = v.z, }; 56 | return vv; 57 | } 58 | 59 | void collisionAvoidanceUpdateSetpointWrap( 60 | collision_avoidance_params_t const *params, 61 | collision_avoidance_state_t *collisionState, 62 | int nOthers, 63 | float const *otherPositions, 64 | setpoint_t *setpoint, sensorData_t const *sensorData, state_t const *state) 65 | { 66 | nOthers /= 3; 67 | float *workspace = malloc(sizeof(float) * 7 * (nOthers + 6)); 68 | collisionAvoidanceUpdateSetpointCore( 69 | params, 70 | collisionState, 71 | nOthers, 72 | otherPositions, 73 | workspace, 74 | setpoint, sensorData, state 75 | ); 76 | free(workspace); 77 | } 78 | %} 79 | 80 | 81 | %pythoncode %{ 82 | import numpy as np 83 | %} 84 | 85 | #define COPY_CTOR(structname) \ 86 | structname(struct structname const *x) { \ 87 | struct structname *y = malloc(sizeof(struct structname)); \ 88 | *y = *x; \ 89 | return y; \ 90 | } \ 91 | ~structname() { \ 92 | free($self); \ 93 | } \ 94 | 95 | %extend vec { 96 | COPY_CTOR(vec) 97 | 98 | %pythoncode %{ 99 | def __repr__(self): 100 | return "({}, {}, {})".format(self.x, self.y, self.z) 101 | 102 | def __array__(self): 103 | return np.array([self.x, self.y, self.z]) 104 | 105 | def __len__(self): 106 | return 3 107 | 108 | def __getitem__(self, i): 109 | if 0 <= i and i < 3: 110 | return _cffirmware.vindex(self, i) 111 | else: 112 | raise IndexError("vec index must be in {0, 1, 2}.") 113 | 114 | # Unary operator overloads. 115 | def __neg__(self): 116 | return _cffirmware.vneg(self) 117 | 118 | # Vector-scalar binary operator overloads. 119 | def __rmul__(self, s): 120 | return _cffirmware.vscl(s, self) 121 | 122 | def __div__(self, s): 123 | return self.__truediv__(s) 124 | 125 | def __truediv__(self, s): 126 | return _cffirmware.vdiv(self, s) 127 | 128 | # Vector-vector binary operator overloads. 129 | def __add__(self, other): 130 | return _cffirmware.vadd(self, other) 131 | 132 | def __sub__(self, other): 133 | return _cffirmware.vsub(self, other) 134 | %} 135 | }; 136 | 137 | %extend traj_eval { 138 | COPY_CTOR(traj_eval) 139 | }; 140 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/cfsim/setup.py: -------------------------------------------------------------------------------- 1 | """Compiles the cffirmware C extension.""" 2 | 3 | from distutils.core import setup, Extension 4 | import os 5 | 6 | import numpy as np 7 | 8 | 9 | fw_dir = "../../../../../../crazyflie-firmware" 10 | include = [ 11 | os.path.join(fw_dir, "src/modules/interface"), 12 | os.path.join(fw_dir, "src/hal/interface"), 13 | os.path.join(fw_dir, "src/utils/interface/lighthouse"), 14 | np.get_include(), 15 | ] 16 | 17 | modules = [ 18 | "collision_avoidance.c", 19 | "planner.c", 20 | "pptraj.c", 21 | "pptraj_compressed.c", 22 | ] 23 | fw_sources = [os.path.join(fw_dir, "src/modules/src", mod) for mod in modules] 24 | 25 | cffirmware = Extension( 26 | "_cffirmware", 27 | include_dirs=include, 28 | sources=fw_sources + ["cffirmware_wrap.c"], 29 | extra_compile_args=[ 30 | "-O3", 31 | "-D__fp16=uint16_t", 32 | ], 33 | ) 34 | 35 | setup(name="cffirmware", version="1.0", ext_modules=[cffirmware]) 36 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/crazyflie.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/crazyflie.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/crazyflieSim.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/crazyflieSim.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/crazyflie_forward_model.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/crazyflie_forward_model.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/crazyswarm_py.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/crazyswarm_py.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/msg/_FullState.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/msg/_FullState.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/msg/_GenericLogData.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/msg/_GenericLogData.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/msg/_Position.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/msg/_Position.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/msg/_TrajectoryPolynomialPiece.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/msg/_TrajectoryPolynomialPiece.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/msg/_VelocityWorld.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/msg/_VelocityWorld.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/msg/__init__.py: -------------------------------------------------------------------------------- 1 | from ._Position import * 2 | from ._TrajectoryPolynomialPiece import * 3 | from ._VelocityWorld import * 4 | from ._FullState import * 5 | from ._GenericLogData import * 6 | -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/msg/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/crazyswarm_ros/scripts/pycrazyswarm/msg/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/crazyswarm_ros/scripts/pycrazyswarm/srv/_AddInts.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from hello_vscode/AddIntsRequest.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class AddIntsRequest(genpy.Message): 11 | _md5sum = "c68f3979e1a90aac7d1c75a1096d1e60" 12 | _type = "hello_vscode/AddIntsRequest" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# 客户端请求时发送的两个数字 15 | int32 num1 16 | int32 num2 17 | """ 18 | __slots__ = ['num1','num2'] 19 | _slot_types = ['int32','int32'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | num1,num2 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(AddIntsRequest, self).__init__(*args, **kwds) 37 | # message fields cannot be None, assign default values for those that are 38 | if self.num1 is None: 39 | self.num1 = 0 40 | if self.num2 is None: 41 | self.num2 = 0 42 | else: 43 | self.num1 = 0 44 | self.num2 = 0 45 | 46 | def _get_types(self): 47 | """ 48 | internal API method 49 | """ 50 | return self._slot_types 51 | 52 | def serialize(self, buff): 53 | """ 54 | serialize message into buffer 55 | :param buff: buffer, ``StringIO`` 56 | """ 57 | try: 58 | _x = self 59 | buff.write(_get_struct_2i().pack(_x.num1, _x.num2)) 60 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 61 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 62 | 63 | def deserialize(self, str): 64 | """ 65 | unpack serialized message in str into this message instance 66 | :param str: byte array of serialized message, ``str`` 67 | """ 68 | if python3: 69 | codecs.lookup_error("rosmsg").msg_type = self._type 70 | try: 71 | end = 0 72 | _x = self 73 | start = end 74 | end += 8 75 | (_x.num1, _x.num2,) = _get_struct_2i().unpack(str[start:end]) 76 | return self 77 | except struct.error as e: 78 | raise genpy.DeserializationError(e) # most likely buffer underfill 79 | 80 | 81 | def serialize_numpy(self, buff, numpy): 82 | """ 83 | serialize message with numpy array types into buffer 84 | :param buff: buffer, ``StringIO`` 85 | :param numpy: numpy python module 86 | """ 87 | try: 88 | _x = self 89 | buff.write(_get_struct_2i().pack(_x.num1, _x.num2)) 90 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 91 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 92 | 93 | def deserialize_numpy(self, str, numpy): 94 | """ 95 | unpack serialized message in str into this message instance using numpy for array types 96 | :param str: byte array of serialized message, ``str`` 97 | :param numpy: numpy python module 98 | """ 99 | if python3: 100 | codecs.lookup_error("rosmsg").msg_type = self._type 101 | try: 102 | end = 0 103 | _x = self 104 | start = end 105 | end += 8 106 | (_x.num1, _x.num2,) = _get_struct_2i().unpack(str[start:end]) 107 | return self 108 | except struct.error as e: 109 | raise genpy.DeserializationError(e) # most likely buffer underfill 110 | 111 | _struct_I = genpy.struct_I 112 | def _get_struct_I(): 113 | global _struct_I 114 | return _struct_I 115 | _struct_2i = None 116 | def _get_struct_2i(): 117 | global _struct_2i 118 | if _struct_2i is None: 119 | _struct_2i = struct.Struct("<2i") 120 | return _struct_2i 121 | # This Python file uses the following encoding: utf-8 122 | """autogenerated by genpy from hello_vscode/AddIntsResponse.msg. Do not edit.""" 123 | import codecs 124 | import sys 125 | python3 = True if sys.hexversion > 0x03000000 else False 126 | import genpy 127 | import struct 128 | 129 | 130 | class AddIntsResponse(genpy.Message): 131 | _md5sum = "0ba699c25c9418c0366f3595c0c8e8ec" 132 | _type = "hello_vscode/AddIntsResponse" 133 | _has_header = False # flag to mark the presence of a Header object 134 | _full_text = """# 服务器响应发送的数据 135 | int32 sum 136 | """ 137 | __slots__ = ['sum'] 138 | _slot_types = ['int32'] 139 | 140 | def __init__(self, *args, **kwds): 141 | """ 142 | Constructor. Any message fields that are implicitly/explicitly 143 | set to None will be assigned a default value. The recommend 144 | use is keyword arguments as this is more robust to future message 145 | changes. You cannot mix in-order arguments and keyword arguments. 146 | 147 | The available fields are: 148 | sum 149 | 150 | :param args: complete set of field values, in .msg order 151 | :param kwds: use keyword arguments corresponding to message field names 152 | to set specific fields. 153 | """ 154 | if args or kwds: 155 | super(AddIntsResponse, self).__init__(*args, **kwds) 156 | # message fields cannot be None, assign default values for those that are 157 | if self.sum is None: 158 | self.sum = 0 159 | else: 160 | self.sum = 0 161 | 162 | def _get_types(self): 163 | """ 164 | internal API method 165 | """ 166 | return self._slot_types 167 | 168 | def serialize(self, buff): 169 | """ 170 | serialize message into buffer 171 | :param buff: buffer, ``StringIO`` 172 | """ 173 | try: 174 | _x = self.sum 175 | buff.write(_get_struct_i().pack(_x)) 176 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 177 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 178 | 179 | def deserialize(self, str): 180 | """ 181 | unpack serialized message in str into this message instance 182 | :param str: byte array of serialized message, ``str`` 183 | """ 184 | if python3: 185 | codecs.lookup_error("rosmsg").msg_type = self._type 186 | try: 187 | end = 0 188 | start = end 189 | end += 4 190 | (self.sum,) = _get_struct_i().unpack(str[start:end]) 191 | return self 192 | except struct.error as e: 193 | raise genpy.DeserializationError(e) # most likely buffer underfill 194 | 195 | 196 | def serialize_numpy(self, buff, numpy): 197 | """ 198 | serialize message with numpy array types into buffer 199 | :param buff: buffer, ``StringIO`` 200 | :param numpy: numpy python module 201 | """ 202 | try: 203 | _x = self.sum 204 | buff.write(_get_struct_i().pack(_x)) 205 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 206 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 207 | 208 | def deserialize_numpy(self, str, numpy): 209 | """ 210 | unpack serialized message in str into this message instance using numpy for array types 211 | :param str: byte array of serialized message, ``str`` 212 | :param numpy: numpy python module 213 | """ 214 | if python3: 215 | codecs.lookup_error("rosmsg").msg_type = self._type 216 | try: 217 | end = 0 218 | start = end 219 | end += 4 220 | (self.sum,) = _get_struct_i().unpack(str[start:end]) 221 | return self 222 | except struct.error as e: 223 | raise genpy.DeserializationError(e) # most likely buffer underfill 224 | 225 | _struct_I = genpy.struct_I 226 | def _get_struct_I(): 227 | global _struct_I 228 | return _struct_I 229 | _struct_i = None 230 | def _get_struct_i(): 231 | global _struct_i 232 | if _struct_i is None: 233 | _struct_i = struct.Struct(" 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/swarm_brain/launch/start_crazyswarm_go_circle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/swarm_brain/launch/start_wheelswarm_swarm_brain.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/swarm_brain/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | swarm_brain 4 | 0.0.0 5 | The swarm_brain package 6 | 7 | 8 | 9 | 10 | amov 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | geometry_msgs 59 | message_generation 60 | roscpp 61 | rospy 62 | std_msgs 63 | geometry_msgs 64 | message_runtime 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/__pycache__/uav_trajectory.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/__pycache__/uav_trajectory.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/crazyflie_gcs.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from os.path import abspath, join, dirname 3 | sys.path.insert(0, join(abspath(dirname(__file__)))) 4 | 5 | import rospy 6 | from pycrazyswarm.srv import * 7 | import sys 8 | from rospy import Duration 9 | from geometry_msgs.msg import Point 10 | import random 11 | import uav_trajectory 12 | from pycrazyswarm.msg import * 13 | import tf_conversions 14 | 15 | quad_num = len(rospy.get_param('crazyflies')) 16 | 17 | def take_off(height): 18 | # 执行起飞操作的代码 19 | for id in range(1,quad_num+1): 20 | req = TakeoffRequest(id, 0, height, Duration(2.5)) 21 | takeoff_client.call(req) 22 | 23 | rospy.loginfo("飞机起飞") 24 | 25 | 26 | def land(): 27 | # 执行降落操作的代码 28 | for id in range(1,quad_num+1): 29 | req = LandRequest(id, 0, 0.04, Duration(2.5)) 30 | land_client.call(req) 31 | 32 | rospy.loginfo("飞机降落") 33 | 34 | def goto(): 35 | 36 | for id in range(1, quad_num+1): 37 | goal = Point() 38 | goal.x = random.uniform(-4, 4) 39 | goal.y = random.uniform(-4, 4) 40 | goal.z = 2.0 41 | req = GoToRequest(id, 0, False, goal, 0.0, Duration(2.5)) 42 | goto_client.call(req) 43 | 44 | rospy.loginfo("飞机到达指定位置") 45 | 46 | def upload_traj(traj1): 47 | 48 | for id in range(1,quad_num+1): 49 | req = UploadTrajectoryRequest(id, 0, 0, traj1.polynomials) 50 | upload_traj_client.call(req) 51 | 52 | rospy.loginfo("上传飞机轨迹") 53 | 54 | 55 | def start_traj(): 56 | 57 | for id in range(1,quad_num+1): 58 | req = StartTrajectoryRequest(id, 0, 0, 1.0, False, True) 59 | start_traj_client.call(req) 60 | 61 | rospy.loginfo("按轨迹飞行") 62 | 63 | 64 | 65 | def cmd_full_state(): 66 | pos = [3.0,3.0,1.5] 67 | vel = [1.0,1.0,0.0] 68 | acc = [0.0,0.0,0.0] 69 | yaw = 0.0 70 | omega = [0.0,0.0,0.0] 71 | cmdFullStateMsg.header.stamp = rospy.Time.now() 72 | cmdFullStateMsg.header.seq += 1 73 | cmdFullStateMsg.pose.position.x = pos[0] 74 | cmdFullStateMsg.pose.position.y = pos[1] 75 | cmdFullStateMsg.pose.position.z = pos[2] 76 | cmdFullStateMsg.twist.linear.x = vel[0] 77 | cmdFullStateMsg.twist.linear.y = vel[1] 78 | cmdFullStateMsg.twist.linear.z = vel[2] 79 | cmdFullStateMsg.acc.x = acc[0] 80 | cmdFullStateMsg.acc.y = acc[1] 81 | cmdFullStateMsg.acc.z = acc[2] 82 | cmdFullStateMsg.pose.orientation = geometry_msgs.msg.Quaternion( 83 | *tf_conversions.transformations.quaternion_from_euler(0, 0, yaw)) 84 | cmdFullStateMsg.twist.angular.x = omega[0] 85 | cmdFullStateMsg.twist.angular.y = omega[1] 86 | cmdFullStateMsg.twist.angular.z = omega[2] 87 | 88 | cmdFullStatePublisher.publish(cmdFullStateMsg) 89 | 90 | rospy.loginfo("精准飞行") 91 | 92 | def cmd_pos(): 93 | pos = [3.0, 3.0, 1.5] 94 | yaw = 0.0 95 | cmdPositionMsg.header.stamp = rospy.Time.now() 96 | cmdPositionMsg.header.seq += 1 97 | cmdPositionMsg.x = pos[0] 98 | cmdPositionMsg.y = pos[1] 99 | cmdPositionMsg.z = pos[2] 100 | cmdPositionMsg.yaw = yaw 101 | cmdPositionPublisher.publish(cmdPositionMsg) 102 | 103 | rospy.loginfo("向指定位置飞行-低级") 104 | 105 | if __name__ == "__main__": 106 | rospy.init_node("Crazyswarm_GCS") 107 | 108 | traj1 = uav_trajectory.Trajectory() 109 | traj1.loadcsv(dirname(__file__)+"/figure8.csv") 110 | 111 | takeoff_client = rospy.ServiceProxy("Takeoff", Takeoff) 112 | takeoff_client.wait_for_service() 113 | 114 | land_client = rospy.ServiceProxy("Land", Land) 115 | land_client.wait_for_service() 116 | 117 | goto_client = rospy.ServiceProxy("GoTo", GoTo) 118 | goto_client.wait_for_service() 119 | 120 | upload_traj_client = rospy.ServiceProxy("UploadTrajectory", UploadTrajectory) 121 | upload_traj_client.wait_for_service() 122 | 123 | start_traj_client = rospy.ServiceProxy("StartTrajectory", StartTrajectory) 124 | start_traj_client.wait_for_service() 125 | 126 | cmdVelocityWorldPublisher = rospy.Publisher("/cf1/cmd_velocity_world", VelocityWorld, queue_size=1) 127 | cmdVelocityWorldMsg = VelocityWorld() 128 | cmdVelocityWorldMsg.header.seq = 0 129 | cmdVelocityWorldMsg.header.frame_id = "/cf1" 130 | 131 | cmdFullStatePublisher = rospy.Publisher("/cf1/cmd_full_state", FullState, queue_size=1) 132 | cmdFullStateMsg = FullState() 133 | cmdFullStateMsg.header.seq = 0 134 | cmdFullStateMsg.header.frame_id = "/cf1" 135 | 136 | cmdPositionPublisher = rospy.Publisher("cf1/cmd_position", Position, queue_size=1) 137 | cmdPositionMsg = Position() 138 | cmdPositionMsg.header.seq = 0 139 | cmdPositionMsg.header.frame_id = "/cf1" 140 | 141 | while not rospy.is_shutdown(): 142 | command = input("请输入指令（take_off/land/goto/upload/fly/quit）：") # 等待用户输入指令 143 | 144 | if command == "take_off": 145 | height = 0.5 # 提示用户输入起飞高度 146 | take_off(height) 147 | elif command == "land": 148 | land() 149 | elif command == "goto": 150 | goto() 151 | elif command == "upload": 152 | upload_traj(traj1) 153 | elif command == "fly": 154 | start_traj() 155 | elif command == "full": 156 | cmd_full_state() 157 | elif command == "pos": 158 | cmd_pos() 159 | elif command == "quit": 160 | break 161 | else: 162 | rospy.logwarn("无效的指令") -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/figure8.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.050000,0.000000,-0.000000,0.000000,-0.000000,0.830443,-0.276140,-0.384219,0.180493,-0.000000,0.000000,-0.000000,0.000000,-1.356107,0.688430,0.587426,-0.329106,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 0.710000,0.396058,0.918033,0.128965,-0.773546,0.339704,0.034310,-0.026417,-0.030049,-0.445604,-0.684403,0.888433,1.493630,-1.361618,-0.139316,0.158875,0.095799,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 0.620000,0.922409,0.405715,-0.582968,-0.092188,-0.114670,0.101046,0.075834,-0.037926,-0.291165,0.967514,0.421451,-1.086348,0.545211,0.030109,-0.050046,-0.068177,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 0.700000,0.923174,-0.431533,-0.682975,0.177173,0.319468,-0.043852,-0.111269,0.023166,0.289869,0.724722,-0.512011,-0.209623,-0.218710,0.108797,0.128756,-0.055461,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 0.560000,0.405364,-0.834716,0.158939,0.288175,-0.373738,-0.054995,0.036090,0.078627,0.450742,-0.385534,-0.954089,0.128288,0.442620,0.055630,-0.060142,-0.076163,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 0.560000,0.001062,-0.646270,-0.012560,-0.324065,0.125327,0.119738,0.034567,-0.063130,0.001593,-1.031457,0.015159,0.820816,-0.152665,-0.130729,-0.045679,0.080444,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 0.700000,-0.402804,-0.820508,-0.132914,0.236278,0.235164,-0.053551,-0.088687,0.031253,-0.449354,-0.411507,0.902946,0.185335,-0.239125,-0.041696,0.016857,0.016709,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 0.620000,-0.921641,-0.464596,0.661875,0.286582,-0.228921,-0.051987,0.004669,0.038463,-0.292459,0.777682,0.565788,-0.432472,-0.060568,-0.082048,-0.009439,0.041158,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 0.710000,-0.923935,0.447832,0.627381,-0.259808,-0.042325,-0.032258,0.001420,0.005294,0.288570,0.873350,-0.515586,-0.730207,-0.026023,0.288755,0.215678,-0.148061,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | 1.053185,-0.398611,0.850510,-0.144007,-0.485368,-0.079781,0.176330,0.234482,-0.153567,0.447039,-0.532729,-0.855023,0.878509,0.775168,-0.391051,-0.713519,0.391628,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 12 | -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/go_circle.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from os.path import abspath, join, dirname 3 | sys.path.insert(0, join(abspath(dirname(__file__)))) 4 | 5 | import rospy 6 | from pycrazyswarm.srv import * 7 | import sys 8 | from rospy import Duration 9 | from geometry_msgs.msg import Point 10 | import random 11 | import uav_trajectory 12 | from pycrazyswarm.msg import * 13 | import tf_conversions 14 | import numpy as np 15 | from threading import Thread 16 | # from pycrazyswarm import Crazyswarm 17 | import re 18 | import math 19 | 20 | 21 | 22 | Z = 1.0 23 | sleepRate = 30 24 | quad_num = len(rospy.get_param('crazyflies')) 25 | cf_swarm_state = dict() 26 | cf_swarm_publishers = dict() 27 | 28 | for id in range(1, quad_num+1): 29 | topic_cmd_velocity = "/cf" + str(id) + "/cmd_velocity_world" 30 | pub = rospy.Publisher(topic_cmd_velocity, VelocityWorld, queue_size=10) 31 | cmdVelocityWorldMsg = VelocityWorld() 32 | cmdVelocityWorldMsg.header.seq = 0 33 | cmdVelocityWorldMsg.header.frame_id = "/cf" + str(id) 34 | cf_swarm_publishers[id] = [pub, cmdVelocityWorldMsg] 35 | 36 | def circle_pos_calc(num_cf, center_pt, radius): 37 | goals = np.zeros((3,num_cf)) 38 | 39 | for i in range(num_cf): 40 | goals[0,i] = center_pt[0] + radius * math.cos(i * 2 * math.pi / num_cf) 41 | goals[1,i] = center_pt[1] + radius * math.sin(i * 2 * math.pi / num_cf) 42 | goals[2,i] = center_pt[2] 43 | 44 | return goals 45 | 46 | def take_off(height): 47 | # 执行起飞操作的代码 48 | for id in range(1,quad_num+1): 49 | req = TakeoffRequest(id, 0, height, Duration(2.5)) 50 | takeoff_client.call(req) 51 | 52 | # rospy.loginfo("飞机起飞") 53 | 54 | def land(): 55 | # 执行降落操作的代码 56 | for id in range(1,quad_num+1): 57 | req = LandRequest(id, 0, 0.04, Duration(2.5)) 58 | land_client.call(req) 59 | 60 | rospy.loginfo("飞机降落") 61 | 62 | def goto(): 63 | center_pt = np.array([0.0, 0.0, 1.5]) 64 | radius = 2.0 65 | goals = circle_pos_calc(quad_num, center_pt, radius) 66 | for id in range(1, quad_num+1): 67 | goal = Point() 68 | goal.x = goals[0,id-1] 69 | goal.y = goals[1,id-1] 70 | goal.z = goals[2,id-1] 71 | req = GoToRequest(id, 0, False, goal, 0.0, Duration(2.5)) 72 | goto_client.call(req) 73 | 74 | rospy.loginfo("飞机到达指定位置") 75 | 76 | def subPosition_CallBack(data): 77 | cf_id = int(re.findall(r'\d+', data.header.frame_id)[0]) 78 | cf_swarm_state[cf_id] = data 79 | # print(data) 80 | 81 | def handler1(): 82 | for id in range(1,quad_num+1): 83 | topic_pos = "/cf" + str(id) + "/position" 84 | rospy.Subscriber(topic_pos, Position, subPosition_CallBack) 85 | 86 | rospy.spin() 87 | 88 | def handler2(start_time): 89 | totalTime = 3.0 90 | center_circle = np.array([0.0, 0.0, 1.5]) 91 | omega = 2 * np.pi / totalTime 92 | theta_delta = 2*math.pi/quad_num 93 | radius = 2.0 94 | kPosition = 1.0 95 | rate = rospy.Rate(10) 96 | goto_flag = False 97 | take_off_flag = False 98 | land_flag = False 99 | while not rospy.is_shutdown(): 100 | elapsed_time = (rospy.Time.now() - start_time).to_sec() 101 | 102 | if elapsed_time < 5: 103 | if not take_off_flag: 104 | height = 1.5 # 提示用户输入起飞高度 105 | take_off(height) 106 | take_off_flag = True 107 | elif elapsed_time < 10: 108 | if not goto_flag: 109 | goto() 110 | goto_flag = True 111 | elif elapsed_time < 30: 112 | # print("circle") 113 | for cf_id, state in cf_swarm_state.items(): 114 | # print("cf_id:", cf_id) 115 | theta = (cf_id) * theta_delta 116 | 117 | vx = -radius * omega * np.sin(omega * elapsed_time + theta) 118 | vy = radius * omega * np.cos(omega * elapsed_time + theta) 119 | desiredPos = center_circle + radius * np.array( 120 | [np.cos(omega * elapsed_time + theta), np.sin(omega * elapsed_time + theta), 0]) 121 | errorX = desiredPos - np.array([state.x, state.y, state.z]) 122 | 123 | pub, msg = cf_swarm_publishers[cf_id] 124 | msg.header.stamp = rospy.Time.now() 125 | msg.header.seq += 1 126 | msg.vel.x = vx + kPosition * errorX[0] 127 | msg.vel.y = vy + kPosition * errorX[1] 128 | msg.vel.z = kPosition * errorX[2] 129 | msg.yawRate = 0.0 130 | pub.publish(msg) 131 | 132 | rate.sleep() 133 | else: 134 | if not land_flag: 135 | land() 136 | land_flag = True 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | if __name__ == "__main__": 146 | rospy.init_node("Crazyswarm_GoCircle") 147 | 148 | startTime = rospy.Time.now() 149 | 150 | traj1 = uav_trajectory.Trajectory() 151 | traj1.loadcsv(dirname(__file__)+"/figure8.csv") 152 | 153 | takeoff_client = rospy.ServiceProxy("Takeoff", Takeoff) 154 | takeoff_client.wait_for_service() 155 | 156 | land_client = rospy.ServiceProxy("Land", Land) 157 | land_client.wait_for_service() 158 | 159 | goto_client = rospy.ServiceProxy("GoTo", GoTo) 160 | goto_client.wait_for_service() 161 | 162 | upload_traj_client = rospy.ServiceProxy("UploadTrajectory", UploadTrajectory) 163 | upload_traj_client.wait_for_service() 164 | 165 | start_traj_client = rospy.ServiceProxy("StartTrajectory", StartTrajectory) 166 | start_traj_client.wait_for_service() 167 | 168 | t1 = Thread(target=handler1, args=()) 169 | t2 = Thread(target=handler2, args=(startTime,)) 170 | 171 | t1.start() 172 | t2.start() -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/pycrazyswarm/__init__.py -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/pycrazyswarm/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/msg/_GenericLogData.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from hello_vscode/GenericLogData.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | import std_msgs.msg 10 | 11 | class GenericLogData(genpy.Message): 12 | _md5sum = "bfa79f36371fee74e53d96afde61049b" 13 | _type = "hello_vscode/GenericLogData" 14 | _has_header = True # flag to mark the presence of a Header object 15 | _full_text = """Header header 16 | float64[] values 17 | 18 | ================================================================================ 19 | MSG: std_msgs/Header 20 | # Standard metadata for higher-level stamped data types. 21 | # This is generally used to communicate timestamped data 22 | # in a particular coordinate frame. 23 | # 24 | # sequence ID: consecutively increasing ID 25 | uint32 seq 26 | #Two-integer timestamp that is expressed as: 27 | # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') 28 | # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') 29 | # time-handling sugar is provided by the client library 30 | time stamp 31 | #Frame this data is associated with 32 | string frame_id 33 | """ 34 | __slots__ = ['header','values'] 35 | _slot_types = ['std_msgs/Header','float64[]'] 36 | 37 | def __init__(self, *args, **kwds): 38 | """ 39 | Constructor. Any message fields that are implicitly/explicitly 40 | set to None will be assigned a default value. The recommend 41 | use is keyword arguments as this is more robust to future message 42 | changes. You cannot mix in-order arguments and keyword arguments. 43 | 44 | The available fields are: 45 | header,values 46 | 47 | :param args: complete set of field values, in .msg order 48 | :param kwds: use keyword arguments corresponding to message field names 49 | to set specific fields. 50 | """ 51 | if args or kwds: 52 | super(GenericLogData, self).__init__(*args, **kwds) 53 | # message fields cannot be None, assign default values for those that are 54 | if self.header is None: 55 | self.header = std_msgs.msg.Header() 56 | if self.values is None: 57 | self.values = [] 58 | else: 59 | self.header = std_msgs.msg.Header() 60 | self.values = [] 61 | 62 | def _get_types(self): 63 | """ 64 | internal API method 65 | """ 66 | return self._slot_types 67 | 68 | def serialize(self, buff): 69 | """ 70 | serialize message into buffer 71 | :param buff: buffer, ``StringIO`` 72 | """ 73 | try: 74 | _x = self 75 | buff.write(_get_struct_3I().pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 76 | _x = self.header.frame_id 77 | length = len(_x) 78 | if python3 or type(_x) == unicode: 79 | _x = _x.encode('utf-8') 80 | length = len(_x) 81 | buff.write(struct.Struct(' 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | import std_msgs.msg 10 | 11 | class Position(genpy.Message): 12 | _md5sum = "fb9bb23ad995963ed72488318d105adf" 13 | _type = "hello_vscode/Position" 14 | _has_header = True # flag to mark the presence of a Header object 15 | _full_text = """Header header 16 | float32 x 17 | float32 y 18 | float32 z 19 | float32 yaw 20 | 21 | ================================================================================ 22 | MSG: std_msgs/Header 23 | # Standard metadata for higher-level stamped data types. 24 | # This is generally used to communicate timestamped data 25 | # in a particular coordinate frame. 26 | # 27 | # sequence ID: consecutively increasing ID 28 | uint32 seq 29 | #Two-integer timestamp that is expressed as: 30 | # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') 31 | # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') 32 | # time-handling sugar is provided by the client library 33 | time stamp 34 | #Frame this data is associated with 35 | string frame_id 36 | """ 37 | __slots__ = ['header','x','y','z','yaw'] 38 | _slot_types = ['std_msgs/Header','float32','float32','float32','float32'] 39 | 40 | def __init__(self, *args, **kwds): 41 | """ 42 | Constructor. Any message fields that are implicitly/explicitly 43 | set to None will be assigned a default value. The recommend 44 | use is keyword arguments as this is more robust to future message 45 | changes. You cannot mix in-order arguments and keyword arguments. 46 | 47 | The available fields are: 48 | header,x,y,z,yaw 49 | 50 | :param args: complete set of field values, in .msg order 51 | :param kwds: use keyword arguments corresponding to message field names 52 | to set specific fields. 53 | """ 54 | if args or kwds: 55 | super(Position, self).__init__(*args, **kwds) 56 | # message fields cannot be None, assign default values for those that are 57 | if self.header is None: 58 | self.header = std_msgs.msg.Header() 59 | if self.x is None: 60 | self.x = 0. 61 | if self.y is None: 62 | self.y = 0. 63 | if self.z is None: 64 | self.z = 0. 65 | if self.yaw is None: 66 | self.yaw = 0. 67 | else: 68 | self.header = std_msgs.msg.Header() 69 | self.x = 0. 70 | self.y = 0. 71 | self.z = 0. 72 | self.yaw = 0. 73 | 74 | def _get_types(self): 75 | """ 76 | internal API method 77 | """ 78 | return self._slot_types 79 | 80 | def serialize(self, buff): 81 | """ 82 | serialize message into buffer 83 | :param buff: buffer, ``StringIO`` 84 | """ 85 | try: 86 | _x = self 87 | buff.write(_get_struct_3I().pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 88 | _x = self.header.frame_id 89 | length = len(_x) 90 | if python3 or type(_x) == unicode: 91 | _x = _x.encode('utf-8') 92 | length = len(_x) 93 | buff.write(struct.Struct(' 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class TrajectoryPolynomialPiece(genpy.Message): 11 | _md5sum = "21147c56ddca6e77dbbf377801086a4d" 12 | _type = "hello_vscode/TrajectoryPolynomialPiece" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# 15 | 16 | float32[] px 17 | float32[] py 18 | float32[] pz 19 | float32[] pyaw 20 | float32 duration""" 21 | __slots__ = ['px','py','pz','pyaw','duration'] 22 | _slot_types = ['float32[]','float32[]','float32[]','float32[]','float32'] 23 | 24 | def __init__(self, *args, **kwds): 25 | """ 26 | Constructor. Any message fields that are implicitly/explicitly 27 | set to None will be assigned a default value. The recommend 28 | use is keyword arguments as this is more robust to future message 29 | changes. You cannot mix in-order arguments and keyword arguments. 30 | 31 | The available fields are: 32 | px,py,pz,pyaw,duration 33 | 34 | :param args: complete set of field values, in .msg order 35 | :param kwds: use keyword arguments corresponding to message field names 36 | to set specific fields. 37 | """ 38 | if args or kwds: 39 | super(TrajectoryPolynomialPiece, self).__init__(*args, **kwds) 40 | # message fields cannot be None, assign default values for those that are 41 | if self.px is None: 42 | self.px = [] 43 | if self.py is None: 44 | self.py = [] 45 | if self.pz is None: 46 | self.pz = [] 47 | if self.pyaw is None: 48 | self.pyaw = [] 49 | if self.duration is None: 50 | self.duration = 0. 51 | else: 52 | self.px = [] 53 | self.py = [] 54 | self.pz = [] 55 | self.pyaw = [] 56 | self.duration = 0. 57 | 58 | def _get_types(self): 59 | """ 60 | internal API method 61 | """ 62 | return self._slot_types 63 | 64 | def serialize(self, buff): 65 | """ 66 | serialize message into buffer 67 | :param buff: buffer, ``StringIO`` 68 | """ 69 | try: 70 | length = len(self.px) 71 | buff.write(_struct_I.pack(length)) 72 | pattern = '<%sf'%length 73 | buff.write(struct.Struct(pattern).pack(*self.px)) 74 | length = len(self.py) 75 | buff.write(_struct_I.pack(length)) 76 | pattern = '<%sf'%length 77 | buff.write(struct.Struct(pattern).pack(*self.py)) 78 | length = len(self.pz) 79 | buff.write(_struct_I.pack(length)) 80 | pattern = '<%sf'%length 81 | buff.write(struct.Struct(pattern).pack(*self.pz)) 82 | length = len(self.pyaw) 83 | buff.write(_struct_I.pack(length)) 84 | pattern = '<%sf'%length 85 | buff.write(struct.Struct(pattern).pack(*self.pyaw)) 86 | _x = self.duration 87 | buff.write(_get_struct_f().pack(_x)) 88 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 89 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 90 | 91 | def deserialize(self, str): 92 | """ 93 | unpack serialized message in str into this message instance 94 | :param str: byte array of serialized message, ``str`` 95 | """ 96 | if python3: 97 | codecs.lookup_error("rosmsg").msg_type = self._type 98 | try: 99 | end = 0 100 | start = end 101 | end += 4 102 | (length,) = _struct_I.unpack(str[start:end]) 103 | pattern = '<%sf'%length 104 | start = end 105 | s = struct.Struct(pattern) 106 | end += s.size 107 | self.px = s.unpack(str[start:end]) 108 | start = end 109 | end += 4 110 | (length,) = _struct_I.unpack(str[start:end]) 111 | pattern = '<%sf'%length 112 | start = end 113 | s = struct.Struct(pattern) 114 | end += s.size 115 | self.py = s.unpack(str[start:end]) 116 | start = end 117 | end += 4 118 | (length,) = _struct_I.unpack(str[start:end]) 119 | pattern = '<%sf'%length 120 | start = end 121 | s = struct.Struct(pattern) 122 | end += s.size 123 | self.pz = s.unpack(str[start:end]) 124 | start = end 125 | end += 4 126 | (length,) = _struct_I.unpack(str[start:end]) 127 | pattern = '<%sf'%length 128 | start = end 129 | s = struct.Struct(pattern) 130 | end += s.size 131 | self.pyaw = s.unpack(str[start:end]) 132 | start = end 133 | end += 4 134 | (self.duration,) = _get_struct_f().unpack(str[start:end]) 135 | return self 136 | except struct.error as e: 137 | raise genpy.DeserializationError(e) # most likely buffer underfill 138 | 139 | 140 | def serialize_numpy(self, buff, numpy): 141 | """ 142 | serialize message with numpy array types into buffer 143 | :param buff: buffer, ``StringIO`` 144 | :param numpy: numpy python module 145 | """ 146 | try: 147 | length = len(self.px) 148 | buff.write(_struct_I.pack(length)) 149 | pattern = '<%sf'%length 150 | buff.write(self.px.tostring()) 151 | length = len(self.py) 152 | buff.write(_struct_I.pack(length)) 153 | pattern = '<%sf'%length 154 | buff.write(self.py.tostring()) 155 | length = len(self.pz) 156 | buff.write(_struct_I.pack(length)) 157 | pattern = '<%sf'%length 158 | buff.write(self.pz.tostring()) 159 | length = len(self.pyaw) 160 | buff.write(_struct_I.pack(length)) 161 | pattern = '<%sf'%length 162 | buff.write(self.pyaw.tostring()) 163 | _x = self.duration 164 | buff.write(_get_struct_f().pack(_x)) 165 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 166 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 167 | 168 | def deserialize_numpy(self, str, numpy): 169 | """ 170 | unpack serialized message in str into this message instance using numpy for array types 171 | :param str: byte array of serialized message, ``str`` 172 | :param numpy: numpy python module 173 | """ 174 | if python3: 175 | codecs.lookup_error("rosmsg").msg_type = self._type 176 | try: 177 | end = 0 178 | start = end 179 | end += 4 180 | (length,) = _struct_I.unpack(str[start:end]) 181 | pattern = '<%sf'%length 182 | start = end 183 | s = struct.Struct(pattern) 184 | end += s.size 185 | self.px = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 186 | start = end 187 | end += 4 188 | (length,) = _struct_I.unpack(str[start:end]) 189 | pattern = '<%sf'%length 190 | start = end 191 | s = struct.Struct(pattern) 192 | end += s.size 193 | self.py = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 194 | start = end 195 | end += 4 196 | (length,) = _struct_I.unpack(str[start:end]) 197 | pattern = '<%sf'%length 198 | start = end 199 | s = struct.Struct(pattern) 200 | end += s.size 201 | self.pz = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 202 | start = end 203 | end += 4 204 | (length,) = _struct_I.unpack(str[start:end]) 205 | pattern = '<%sf'%length 206 | start = end 207 | s = struct.Struct(pattern) 208 | end += s.size 209 | self.pyaw = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 210 | start = end 211 | end += 4 212 | (self.duration,) = _get_struct_f().unpack(str[start:end]) 213 | return self 214 | except struct.error as e: 215 | raise genpy.DeserializationError(e) # most likely buffer underfill 216 | 217 | _struct_I = genpy.struct_I 218 | def _get_struct_I(): 219 | global _struct_I 220 | return _struct_I 221 | _struct_f = None 222 | def _get_struct_f(): 223 | global _struct_f 224 | if _struct_f is None: 225 | _struct_f = struct.Struct(" 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | import geometry_msgs.msg 10 | import std_msgs.msg 11 | 12 | class VelocityWorld(genpy.Message): 13 | _md5sum = "5c7894b98f86c9f4dc7d7cb4971ec39d" 14 | _type = "hello_vscode/VelocityWorld" 15 | _has_header = True # flag to mark the presence of a Header object 16 | _full_text = """Header header 17 | geometry_msgs/Vector3 vel 18 | float32 yawRate 19 | ================================================================================ 20 | MSG: std_msgs/Header 21 | # Standard metadata for higher-level stamped data types. 22 | # This is generally used to communicate timestamped data 23 | # in a particular coordinate frame. 24 | # 25 | # sequence ID: consecutively increasing ID 26 | uint32 seq 27 | #Two-integer timestamp that is expressed as: 28 | # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') 29 | # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') 30 | # time-handling sugar is provided by the client library 31 | time stamp 32 | #Frame this data is associated with 33 | string frame_id 34 | 35 | ================================================================================ 36 | MSG: geometry_msgs/Vector3 37 | # This represents a vector in free space. 38 | # It is only meant to represent a direction. Therefore, it does not 39 | # make sense to apply a translation to it (e.g., when applying a 40 | # generic rigid transformation to a Vector3, tf2 will only apply the 41 | # rotation). If you want your data to be translatable too, use the 42 | # geometry_msgs/Point message instead. 43 | 44 | float64 x 45 | float64 y 46 | float64 z""" 47 | __slots__ = ['header','vel','yawRate'] 48 | _slot_types = ['std_msgs/Header','geometry_msgs/Vector3','float32'] 49 | 50 | def __init__(self, *args, **kwds): 51 | """ 52 | Constructor. Any message fields that are implicitly/explicitly 53 | set to None will be assigned a default value. The recommend 54 | use is keyword arguments as this is more robust to future message 55 | changes. You cannot mix in-order arguments and keyword arguments. 56 | 57 | The available fields are: 58 | header,vel,yawRate 59 | 60 | :param args: complete set of field values, in .msg order 61 | :param kwds: use keyword arguments corresponding to message field names 62 | to set specific fields. 63 | """ 64 | if args or kwds: 65 | super(VelocityWorld, self).__init__(*args, **kwds) 66 | # message fields cannot be None, assign default values for those that are 67 | if self.header is None: 68 | self.header = std_msgs.msg.Header() 69 | if self.vel is None: 70 | self.vel = geometry_msgs.msg.Vector3() 71 | if self.yawRate is None: 72 | self.yawRate = 0. 73 | else: 74 | self.header = std_msgs.msg.Header() 75 | self.vel = geometry_msgs.msg.Vector3() 76 | self.yawRate = 0. 77 | 78 | def _get_types(self): 79 | """ 80 | internal API method 81 | """ 82 | return self._slot_types 83 | 84 | def serialize(self, buff): 85 | """ 86 | serialize message into buffer 87 | :param buff: buffer, ``StringIO`` 88 | """ 89 | try: 90 | _x = self 91 | buff.write(_get_struct_3I().pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 92 | _x = self.header.frame_id 93 | length = len(_x) 94 | if python3 or type(_x) == unicode: 95 | _x = _x.encode('utf-8') 96 | length = len(_x) 97 | buff.write(struct.Struct(' 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class AddIntsRequest(genpy.Message): 11 | _md5sum = "c68f3979e1a90aac7d1c75a1096d1e60" 12 | _type = "hello_vscode/AddIntsRequest" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# 客户端请求时发送的两个数字 15 | int32 num1 16 | int32 num2 17 | """ 18 | __slots__ = ['num1','num2'] 19 | _slot_types = ['int32','int32'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | num1,num2 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(AddIntsRequest, self).__init__(*args, **kwds) 37 | # message fields cannot be None, assign default values for those that are 38 | if self.num1 is None: 39 | self.num1 = 0 40 | if self.num2 is None: 41 | self.num2 = 0 42 | else: 43 | self.num1 = 0 44 | self.num2 = 0 45 | 46 | def _get_types(self): 47 | """ 48 | internal API method 49 | """ 50 | return self._slot_types 51 | 52 | def serialize(self, buff): 53 | """ 54 | serialize message into buffer 55 | :param buff: buffer, ``StringIO`` 56 | """ 57 | try: 58 | _x = self 59 | buff.write(_get_struct_2i().pack(_x.num1, _x.num2)) 60 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 61 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 62 | 63 | def deserialize(self, str): 64 | """ 65 | unpack serialized message in str into this message instance 66 | :param str: byte array of serialized message, ``str`` 67 | """ 68 | if python3: 69 | codecs.lookup_error("rosmsg").msg_type = self._type 70 | try: 71 | end = 0 72 | _x = self 73 | start = end 74 | end += 8 75 | (_x.num1, _x.num2,) = _get_struct_2i().unpack(str[start:end]) 76 | return self 77 | except struct.error as e: 78 | raise genpy.DeserializationError(e) # most likely buffer underfill 79 | 80 | 81 | def serialize_numpy(self, buff, numpy): 82 | """ 83 | serialize message with numpy array types into buffer 84 | :param buff: buffer, ``StringIO`` 85 | :param numpy: numpy python module 86 | """ 87 | try: 88 | _x = self 89 | buff.write(_get_struct_2i().pack(_x.num1, _x.num2)) 90 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 91 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 92 | 93 | def deserialize_numpy(self, str, numpy): 94 | """ 95 | unpack serialized message in str into this message instance using numpy for array types 96 | :param str: byte array of serialized message, ``str`` 97 | :param numpy: numpy python module 98 | """ 99 | if python3: 100 | codecs.lookup_error("rosmsg").msg_type = self._type 101 | try: 102 | end = 0 103 | _x = self 104 | start = end 105 | end += 8 106 | (_x.num1, _x.num2,) = _get_struct_2i().unpack(str[start:end]) 107 | return self 108 | except struct.error as e: 109 | raise genpy.DeserializationError(e) # most likely buffer underfill 110 | 111 | _struct_I = genpy.struct_I 112 | def _get_struct_I(): 113 | global _struct_I 114 | return _struct_I 115 | _struct_2i = None 116 | def _get_struct_2i(): 117 | global _struct_2i 118 | if _struct_2i is None: 119 | _struct_2i = struct.Struct("<2i") 120 | return _struct_2i 121 | # This Python file uses the following encoding: utf-8 122 | """autogenerated by genpy from hello_vscode/AddIntsResponse.msg. Do not edit.""" 123 | import codecs 124 | import sys 125 | python3 = True if sys.hexversion > 0x03000000 else False 126 | import genpy 127 | import struct 128 | 129 | 130 | class AddIntsResponse(genpy.Message): 131 | _md5sum = "0ba699c25c9418c0366f3595c0c8e8ec" 132 | _type = "hello_vscode/AddIntsResponse" 133 | _has_header = False # flag to mark the presence of a Header object 134 | _full_text = """# 服务器响应发送的数据 135 | int32 sum 136 | """ 137 | __slots__ = ['sum'] 138 | _slot_types = ['int32'] 139 | 140 | def __init__(self, *args, **kwds): 141 | """ 142 | Constructor. Any message fields that are implicitly/explicitly 143 | set to None will be assigned a default value. The recommend 144 | use is keyword arguments as this is more robust to future message 145 | changes. You cannot mix in-order arguments and keyword arguments. 146 | 147 | The available fields are: 148 | sum 149 | 150 | :param args: complete set of field values, in .msg order 151 | :param kwds: use keyword arguments corresponding to message field names 152 | to set specific fields. 153 | """ 154 | if args or kwds: 155 | super(AddIntsResponse, self).__init__(*args, **kwds) 156 | # message fields cannot be None, assign default values for those that are 157 | if self.sum is None: 158 | self.sum = 0 159 | else: 160 | self.sum = 0 161 | 162 | def _get_types(self): 163 | """ 164 | internal API method 165 | """ 166 | return self._slot_types 167 | 168 | def serialize(self, buff): 169 | """ 170 | serialize message into buffer 171 | :param buff: buffer, ``StringIO`` 172 | """ 173 | try: 174 | _x = self.sum 175 | buff.write(_get_struct_i().pack(_x)) 176 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 177 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 178 | 179 | def deserialize(self, str): 180 | """ 181 | unpack serialized message in str into this message instance 182 | :param str: byte array of serialized message, ``str`` 183 | """ 184 | if python3: 185 | codecs.lookup_error("rosmsg").msg_type = self._type 186 | try: 187 | end = 0 188 | start = end 189 | end += 4 190 | (self.sum,) = _get_struct_i().unpack(str[start:end]) 191 | return self 192 | except struct.error as e: 193 | raise genpy.DeserializationError(e) # most likely buffer underfill 194 | 195 | 196 | def serialize_numpy(self, buff, numpy): 197 | """ 198 | serialize message with numpy array types into buffer 199 | :param buff: buffer, ``StringIO`` 200 | :param numpy: numpy python module 201 | """ 202 | try: 203 | _x = self.sum 204 | buff.write(_get_struct_i().pack(_x)) 205 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 206 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 207 | 208 | def deserialize_numpy(self, str, numpy): 209 | """ 210 | unpack serialized message in str into this message instance using numpy for array types 211 | :param str: byte array of serialized message, ``str`` 212 | :param numpy: numpy python module 213 | """ 214 | if python3: 215 | codecs.lookup_error("rosmsg").msg_type = self._type 216 | try: 217 | end = 0 218 | start = end 219 | end += 4 220 | (self.sum,) = _get_struct_i().unpack(str[start:end]) 221 | return self 222 | except struct.error as e: 223 | raise genpy.DeserializationError(e) # most likely buffer underfill 224 | 225 | _struct_I = genpy.struct_I 226 | def _get_struct_I(): 227 | global _struct_I 228 | return _struct_I 229 | _struct_i = None 230 | def _get_struct_i(): 231 | global _struct_i 232 | if _struct_i is None: 233 | _struct_i = struct.Struct(" 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | import genpy 10 | import geometry_msgs.msg 11 | 12 | class GoToRequest(genpy.Message): 13 | _md5sum = "77da2a52438a6dfe1d698bd7c9109ba7" 14 | _type = "hello_vscode/GoToRequest" 15 | _has_header = False # flag to mark the presence of a Header object 16 | _full_text = """uint8 id 17 | uint8 groupMask 18 | bool relative 19 | geometry_msgs/Point goal 20 | float32 yaw # deg 21 | duration duration 22 | 23 | ================================================================================ 24 | MSG: geometry_msgs/Point 25 | # This contains the position of a point in free space 26 | float64 x 27 | float64 y 28 | float64 z 29 | """ 30 | __slots__ = ['id','groupMask','relative','goal','yaw','duration'] 31 | _slot_types = ['uint8','uint8','bool','geometry_msgs/Point','float32','duration'] 32 | 33 | def __init__(self, *args, **kwds): 34 | """ 35 | Constructor. Any message fields that are implicitly/explicitly 36 | set to None will be assigned a default value. The recommend 37 | use is keyword arguments as this is more robust to future message 38 | changes. You cannot mix in-order arguments and keyword arguments. 39 | 40 | The available fields are: 41 | id,groupMask,relative,goal,yaw,duration 42 | 43 | :param args: complete set of field values, in .msg order 44 | :param kwds: use keyword arguments corresponding to message field names 45 | to set specific fields. 46 | """ 47 | if args or kwds: 48 | super(GoToRequest, self).__init__(*args, **kwds) 49 | # message fields cannot be None, assign default values for those that are 50 | if self.id is None: 51 | self.id = 0 52 | if self.groupMask is None: 53 | self.groupMask = 0 54 | if self.relative is None: 55 | self.relative = False 56 | if self.goal is None: 57 | self.goal = geometry_msgs.msg.Point() 58 | if self.yaw is None: 59 | self.yaw = 0. 60 | if self.duration is None: 61 | self.duration = genpy.Duration() 62 | else: 63 | self.id = 0 64 | self.groupMask = 0 65 | self.relative = False 66 | self.goal = geometry_msgs.msg.Point() 67 | self.yaw = 0. 68 | self.duration = genpy.Duration() 69 | 70 | def _get_types(self): 71 | """ 72 | internal API method 73 | """ 74 | return self._slot_types 75 | 76 | def serialize(self, buff): 77 | """ 78 | serialize message into buffer 79 | :param buff: buffer, ``StringIO`` 80 | """ 81 | try: 82 | _x = self 83 | buff.write(_get_struct_3B3df2i().pack(_x.id, _x.groupMask, _x.relative, _x.goal.x, _x.goal.y, _x.goal.z, _x.yaw, _x.duration.secs, _x.duration.nsecs)) 84 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 85 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 86 | 87 | def deserialize(self, str): 88 | """ 89 | unpack serialized message in str into this message instance 90 | :param str: byte array of serialized message, ``str`` 91 | """ 92 | if python3: 93 | codecs.lookup_error("rosmsg").msg_type = self._type 94 | try: 95 | if self.goal is None: 96 | self.goal = geometry_msgs.msg.Point() 97 | if self.duration is None: 98 | self.duration = genpy.Duration() 99 | end = 0 100 | _x = self 101 | start = end 102 | end += 39 103 | (_x.id, _x.groupMask, _x.relative, _x.goal.x, _x.goal.y, _x.goal.z, _x.yaw, _x.duration.secs, _x.duration.nsecs,) = _get_struct_3B3df2i().unpack(str[start:end]) 104 | self.relative = bool(self.relative) 105 | self.duration.canon() 106 | return self 107 | except struct.error as e: 108 | raise genpy.DeserializationError(e) # most likely buffer underfill 109 | 110 | 111 | def serialize_numpy(self, buff, numpy): 112 | """ 113 | serialize message with numpy array types into buffer 114 | :param buff: buffer, ``StringIO`` 115 | :param numpy: numpy python module 116 | """ 117 | try: 118 | _x = self 119 | buff.write(_get_struct_3B3df2i().pack(_x.id, _x.groupMask, _x.relative, _x.goal.x, _x.goal.y, _x.goal.z, _x.yaw, _x.duration.secs, _x.duration.nsecs)) 120 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 121 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 122 | 123 | def deserialize_numpy(self, str, numpy): 124 | """ 125 | unpack serialized message in str into this message instance using numpy for array types 126 | :param str: byte array of serialized message, ``str`` 127 | :param numpy: numpy python module 128 | """ 129 | if python3: 130 | codecs.lookup_error("rosmsg").msg_type = self._type 131 | try: 132 | if self.goal is None: 133 | self.goal = geometry_msgs.msg.Point() 134 | if self.duration is None: 135 | self.duration = genpy.Duration() 136 | end = 0 137 | _x = self 138 | start = end 139 | end += 39 140 | (_x.id, _x.groupMask, _x.relative, _x.goal.x, _x.goal.y, _x.goal.z, _x.yaw, _x.duration.secs, _x.duration.nsecs,) = _get_struct_3B3df2i().unpack(str[start:end]) 141 | self.relative = bool(self.relative) 142 | self.duration.canon() 143 | return self 144 | except struct.error as e: 145 | raise genpy.DeserializationError(e) # most likely buffer underfill 146 | 147 | _struct_I = genpy.struct_I 148 | def _get_struct_I(): 149 | global _struct_I 150 | return _struct_I 151 | _struct_3B3df2i = None 152 | def _get_struct_3B3df2i(): 153 | global _struct_3B3df2i 154 | if _struct_3B3df2i is None: 155 | _struct_3B3df2i = struct.Struct("<3B3df2i") 156 | return _struct_3B3df2i 157 | # This Python file uses the following encoding: utf-8 158 | """autogenerated by genpy from hello_vscode/GoToResponse.msg. Do not edit.""" 159 | import codecs 160 | import sys 161 | python3 = True if sys.hexversion > 0x03000000 else False 162 | import genpy 163 | import struct 164 | 165 | 166 | class GoToResponse(genpy.Message): 167 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 168 | _type = "hello_vscode/GoToResponse" 169 | _has_header = False # flag to mark the presence of a Header object 170 | _full_text = """ 171 | """ 172 | __slots__ = [] 173 | _slot_types = [] 174 | 175 | def __init__(self, *args, **kwds): 176 | """ 177 | Constructor. Any message fields that are implicitly/explicitly 178 | set to None will be assigned a default value. The recommend 179 | use is keyword arguments as this is more robust to future message 180 | changes. You cannot mix in-order arguments and keyword arguments. 181 | 182 | The available fields are: 183 | 184 | 185 | :param args: complete set of field values, in .msg order 186 | :param kwds: use keyword arguments corresponding to message field names 187 | to set specific fields. 188 | """ 189 | if args or kwds: 190 | super(GoToResponse, self).__init__(*args, **kwds) 191 | 192 | def _get_types(self): 193 | """ 194 | internal API method 195 | """ 196 | return self._slot_types 197 | 198 | def serialize(self, buff): 199 | """ 200 | serialize message into buffer 201 | :param buff: buffer, ``StringIO`` 202 | """ 203 | try: 204 | pass 205 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 206 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 207 | 208 | def deserialize(self, str): 209 | """ 210 | unpack serialized message in str into this message instance 211 | :param str: byte array of serialized message, ``str`` 212 | """ 213 | if python3: 214 | codecs.lookup_error("rosmsg").msg_type = self._type 215 | try: 216 | end = 0 217 | return self 218 | except struct.error as e: 219 | raise genpy.DeserializationError(e) # most likely buffer underfill 220 | 221 | 222 | def serialize_numpy(self, buff, numpy): 223 | """ 224 | serialize message with numpy array types into buffer 225 | :param buff: buffer, ``StringIO`` 226 | :param numpy: numpy python module 227 | """ 228 | try: 229 | pass 230 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 231 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 232 | 233 | def deserialize_numpy(self, str, numpy): 234 | """ 235 | unpack serialized message in str into this message instance using numpy for array types 236 | :param str: byte array of serialized message, ``str`` 237 | :param numpy: numpy python module 238 | """ 239 | if python3: 240 | codecs.lookup_error("rosmsg").msg_type = self._type 241 | try: 242 | end = 0 243 | return self 244 | except struct.error as e: 245 | raise genpy.DeserializationError(e) # most likely buffer underfill 246 | 247 | _struct_I = genpy.struct_I 248 | def _get_struct_I(): 249 | global _struct_I 250 | return _struct_I 251 | class GoTo(object): 252 | _type = 'hello_vscode/GoTo' 253 | _md5sum = '77da2a52438a6dfe1d698bd7c9109ba7' 254 | _request_class = GoToRequest 255 | _response_class = GoToResponse 256 | -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/_Land.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from hello_vscode/LandRequest.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | import genpy 10 | 11 | class LandRequest(genpy.Message): 12 | _md5sum = "351d0c9324942af86fe17feac37b587b" 13 | _type = "hello_vscode/LandRequest" 14 | _has_header = False # flag to mark the presence of a Header object 15 | _full_text = """uint8 id 16 | uint8 groupMask 17 | float32 height 18 | duration duration 19 | """ 20 | __slots__ = ['id','groupMask','height','duration'] 21 | _slot_types = ['uint8','uint8','float32','duration'] 22 | 23 | def __init__(self, *args, **kwds): 24 | """ 25 | Constructor. Any message fields that are implicitly/explicitly 26 | set to None will be assigned a default value. The recommend 27 | use is keyword arguments as this is more robust to future message 28 | changes. You cannot mix in-order arguments and keyword arguments. 29 | 30 | The available fields are: 31 | id,groupMask,height,duration 32 | 33 | :param args: complete set of field values, in .msg order 34 | :param kwds: use keyword arguments corresponding to message field names 35 | to set specific fields. 36 | """ 37 | if args or kwds: 38 | super(LandRequest, self).__init__(*args, **kwds) 39 | # message fields cannot be None, assign default values for those that are 40 | if self.id is None: 41 | self.id = 0 42 | if self.groupMask is None: 43 | self.groupMask = 0 44 | if self.height is None: 45 | self.height = 0. 46 | if self.duration is None: 47 | self.duration = genpy.Duration() 48 | else: 49 | self.id = 0 50 | self.groupMask = 0 51 | self.height = 0. 52 | self.duration = genpy.Duration() 53 | 54 | def _get_types(self): 55 | """ 56 | internal API method 57 | """ 58 | return self._slot_types 59 | 60 | def serialize(self, buff): 61 | """ 62 | serialize message into buffer 63 | :param buff: buffer, ``StringIO`` 64 | """ 65 | try: 66 | _x = self 67 | buff.write(_get_struct_2Bf2i().pack(_x.id, _x.groupMask, _x.height, _x.duration.secs, _x.duration.nsecs)) 68 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 69 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 70 | 71 | def deserialize(self, str): 72 | """ 73 | unpack serialized message in str into this message instance 74 | :param str: byte array of serialized message, ``str`` 75 | """ 76 | if python3: 77 | codecs.lookup_error("rosmsg").msg_type = self._type 78 | try: 79 | if self.duration is None: 80 | self.duration = genpy.Duration() 81 | end = 0 82 | _x = self 83 | start = end 84 | end += 14 85 | (_x.id, _x.groupMask, _x.height, _x.duration.secs, _x.duration.nsecs,) = _get_struct_2Bf2i().unpack(str[start:end]) 86 | self.duration.canon() 87 | return self 88 | except struct.error as e: 89 | raise genpy.DeserializationError(e) # most likely buffer underfill 90 | 91 | 92 | def serialize_numpy(self, buff, numpy): 93 | """ 94 | serialize message with numpy array types into buffer 95 | :param buff: buffer, ``StringIO`` 96 | :param numpy: numpy python module 97 | """ 98 | try: 99 | _x = self 100 | buff.write(_get_struct_2Bf2i().pack(_x.id, _x.groupMask, _x.height, _x.duration.secs, _x.duration.nsecs)) 101 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 102 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 103 | 104 | def deserialize_numpy(self, str, numpy): 105 | """ 106 | unpack serialized message in str into this message instance using numpy for array types 107 | :param str: byte array of serialized message, ``str`` 108 | :param numpy: numpy python module 109 | """ 110 | if python3: 111 | codecs.lookup_error("rosmsg").msg_type = self._type 112 | try: 113 | if self.duration is None: 114 | self.duration = genpy.Duration() 115 | end = 0 116 | _x = self 117 | start = end 118 | end += 14 119 | (_x.id, _x.groupMask, _x.height, _x.duration.secs, _x.duration.nsecs,) = _get_struct_2Bf2i().unpack(str[start:end]) 120 | self.duration.canon() 121 | return self 122 | except struct.error as e: 123 | raise genpy.DeserializationError(e) # most likely buffer underfill 124 | 125 | _struct_I = genpy.struct_I 126 | def _get_struct_I(): 127 | global _struct_I 128 | return _struct_I 129 | _struct_2Bf2i = None 130 | def _get_struct_2Bf2i(): 131 | global _struct_2Bf2i 132 | if _struct_2Bf2i is None: 133 | _struct_2Bf2i = struct.Struct("<2Bf2i") 134 | return _struct_2Bf2i 135 | # This Python file uses the following encoding: utf-8 136 | """autogenerated by genpy from hello_vscode/LandResponse.msg. Do not edit.""" 137 | import codecs 138 | import sys 139 | python3 = True if sys.hexversion > 0x03000000 else False 140 | import genpy 141 | import struct 142 | 143 | 144 | class LandResponse(genpy.Message): 145 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 146 | _type = "hello_vscode/LandResponse" 147 | _has_header = False # flag to mark the presence of a Header object 148 | _full_text = """ 149 | """ 150 | __slots__ = [] 151 | _slot_types = [] 152 | 153 | def __init__(self, *args, **kwds): 154 | """ 155 | Constructor. Any message fields that are implicitly/explicitly 156 | set to None will be assigned a default value. The recommend 157 | use is keyword arguments as this is more robust to future message 158 | changes. You cannot mix in-order arguments and keyword arguments. 159 | 160 | The available fields are: 161 | 162 | 163 | :param args: complete set of field values, in .msg order 164 | :param kwds: use keyword arguments corresponding to message field names 165 | to set specific fields. 166 | """ 167 | if args or kwds: 168 | super(LandResponse, self).__init__(*args, **kwds) 169 | 170 | def _get_types(self): 171 | """ 172 | internal API method 173 | """ 174 | return self._slot_types 175 | 176 | def serialize(self, buff): 177 | """ 178 | serialize message into buffer 179 | :param buff: buffer, ``StringIO`` 180 | """ 181 | try: 182 | pass 183 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 184 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 185 | 186 | def deserialize(self, str): 187 | """ 188 | unpack serialized message in str into this message instance 189 | :param str: byte array of serialized message, ``str`` 190 | """ 191 | if python3: 192 | codecs.lookup_error("rosmsg").msg_type = self._type 193 | try: 194 | end = 0 195 | return self 196 | except struct.error as e: 197 | raise genpy.DeserializationError(e) # most likely buffer underfill 198 | 199 | 200 | def serialize_numpy(self, buff, numpy): 201 | """ 202 | serialize message with numpy array types into buffer 203 | :param buff: buffer, ``StringIO`` 204 | :param numpy: numpy python module 205 | """ 206 | try: 207 | pass 208 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 209 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 210 | 211 | def deserialize_numpy(self, str, numpy): 212 | """ 213 | unpack serialized message in str into this message instance using numpy for array types 214 | :param str: byte array of serialized message, ``str`` 215 | :param numpy: numpy python module 216 | """ 217 | if python3: 218 | codecs.lookup_error("rosmsg").msg_type = self._type 219 | try: 220 | end = 0 221 | return self 222 | except struct.error as e: 223 | raise genpy.DeserializationError(e) # most likely buffer underfill 224 | 225 | _struct_I = genpy.struct_I 226 | def _get_struct_I(): 227 | global _struct_I 228 | return _struct_I 229 | class Land(object): 230 | _type = 'hello_vscode/Land' 231 | _md5sum = '351d0c9324942af86fe17feac37b587b' 232 | _request_class = LandRequest 233 | _response_class = LandResponse 234 | -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/_StartTrajectory.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from hello_vscode/StartTrajectoryRequest.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class StartTrajectoryRequest(genpy.Message): 11 | _md5sum = "e97a7b67fc7fb1e97c2ebdff30f93234" 12 | _type = "hello_vscode/StartTrajectoryRequest" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """uint8 id 15 | uint8 groupMask 16 | uint8 trajectoryId 17 | float32 timescale 18 | bool reversed 19 | bool relative 20 | """ 21 | __slots__ = ['id','groupMask','trajectoryId','timescale','reversed','relative'] 22 | _slot_types = ['uint8','uint8','uint8','float32','bool','bool'] 23 | 24 | def __init__(self, *args, **kwds): 25 | """ 26 | Constructor. Any message fields that are implicitly/explicitly 27 | set to None will be assigned a default value. The recommend 28 | use is keyword arguments as this is more robust to future message 29 | changes. You cannot mix in-order arguments and keyword arguments. 30 | 31 | The available fields are: 32 | id,groupMask,trajectoryId,timescale,reversed,relative 33 | 34 | :param args: complete set of field values, in .msg order 35 | :param kwds: use keyword arguments corresponding to message field names 36 | to set specific fields. 37 | """ 38 | if args or kwds: 39 | super(StartTrajectoryRequest, self).__init__(*args, **kwds) 40 | # message fields cannot be None, assign default values for those that are 41 | if self.id is None: 42 | self.id = 0 43 | if self.groupMask is None: 44 | self.groupMask = 0 45 | if self.trajectoryId is None: 46 | self.trajectoryId = 0 47 | if self.timescale is None: 48 | self.timescale = 0. 49 | if self.reversed is None: 50 | self.reversed = False 51 | if self.relative is None: 52 | self.relative = False 53 | else: 54 | self.id = 0 55 | self.groupMask = 0 56 | self.trajectoryId = 0 57 | self.timescale = 0. 58 | self.reversed = False 59 | self.relative = False 60 | 61 | def _get_types(self): 62 | """ 63 | internal API method 64 | """ 65 | return self._slot_types 66 | 67 | def serialize(self, buff): 68 | """ 69 | serialize message into buffer 70 | :param buff: buffer, ``StringIO`` 71 | """ 72 | try: 73 | _x = self 74 | buff.write(_get_struct_3Bf2B().pack(_x.id, _x.groupMask, _x.trajectoryId, _x.timescale, _x.reversed, _x.relative)) 75 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 76 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 77 | 78 | def deserialize(self, str): 79 | """ 80 | unpack serialized message in str into this message instance 81 | :param str: byte array of serialized message, ``str`` 82 | """ 83 | if python3: 84 | codecs.lookup_error("rosmsg").msg_type = self._type 85 | try: 86 | end = 0 87 | _x = self 88 | start = end 89 | end += 9 90 | (_x.id, _x.groupMask, _x.trajectoryId, _x.timescale, _x.reversed, _x.relative,) = _get_struct_3Bf2B().unpack(str[start:end]) 91 | self.reversed = bool(self.reversed) 92 | self.relative = bool(self.relative) 93 | return self 94 | except struct.error as e: 95 | raise genpy.DeserializationError(e) # most likely buffer underfill 96 | 97 | 98 | def serialize_numpy(self, buff, numpy): 99 | """ 100 | serialize message with numpy array types into buffer 101 | :param buff: buffer, ``StringIO`` 102 | :param numpy: numpy python module 103 | """ 104 | try: 105 | _x = self 106 | buff.write(_get_struct_3Bf2B().pack(_x.id, _x.groupMask, _x.trajectoryId, _x.timescale, _x.reversed, _x.relative)) 107 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 108 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 109 | 110 | def deserialize_numpy(self, str, numpy): 111 | """ 112 | unpack serialized message in str into this message instance using numpy for array types 113 | :param str: byte array of serialized message, ``str`` 114 | :param numpy: numpy python module 115 | """ 116 | if python3: 117 | codecs.lookup_error("rosmsg").msg_type = self._type 118 | try: 119 | end = 0 120 | _x = self 121 | start = end 122 | end += 9 123 | (_x.id, _x.groupMask, _x.trajectoryId, _x.timescale, _x.reversed, _x.relative,) = _get_struct_3Bf2B().unpack(str[start:end]) 124 | self.reversed = bool(self.reversed) 125 | self.relative = bool(self.relative) 126 | return self 127 | except struct.error as e: 128 | raise genpy.DeserializationError(e) # most likely buffer underfill 129 | 130 | _struct_I = genpy.struct_I 131 | def _get_struct_I(): 132 | global _struct_I 133 | return _struct_I 134 | _struct_3Bf2B = None 135 | def _get_struct_3Bf2B(): 136 | global _struct_3Bf2B 137 | if _struct_3Bf2B is None: 138 | _struct_3Bf2B = struct.Struct("<3Bf2B") 139 | return _struct_3Bf2B 140 | # This Python file uses the following encoding: utf-8 141 | """autogenerated by genpy from hello_vscode/StartTrajectoryResponse.msg. Do not edit.""" 142 | import codecs 143 | import sys 144 | python3 = True if sys.hexversion > 0x03000000 else False 145 | import genpy 146 | import struct 147 | 148 | 149 | class StartTrajectoryResponse(genpy.Message): 150 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 151 | _type = "hello_vscode/StartTrajectoryResponse" 152 | _has_header = False # flag to mark the presence of a Header object 153 | _full_text = """ 154 | """ 155 | __slots__ = [] 156 | _slot_types = [] 157 | 158 | def __init__(self, *args, **kwds): 159 | """ 160 | Constructor. Any message fields that are implicitly/explicitly 161 | set to None will be assigned a default value. The recommend 162 | use is keyword arguments as this is more robust to future message 163 | changes. You cannot mix in-order arguments and keyword arguments. 164 | 165 | The available fields are: 166 | 167 | 168 | :param args: complete set of field values, in .msg order 169 | :param kwds: use keyword arguments corresponding to message field names 170 | to set specific fields. 171 | """ 172 | if args or kwds: 173 | super(StartTrajectoryResponse, self).__init__(*args, **kwds) 174 | 175 | def _get_types(self): 176 | """ 177 | internal API method 178 | """ 179 | return self._slot_types 180 | 181 | def serialize(self, buff): 182 | """ 183 | serialize message into buffer 184 | :param buff: buffer, ``StringIO`` 185 | """ 186 | try: 187 | pass 188 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 189 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 190 | 191 | def deserialize(self, str): 192 | """ 193 | unpack serialized message in str into this message instance 194 | :param str: byte array of serialized message, ``str`` 195 | """ 196 | if python3: 197 | codecs.lookup_error("rosmsg").msg_type = self._type 198 | try: 199 | end = 0 200 | return self 201 | except struct.error as e: 202 | raise genpy.DeserializationError(e) # most likely buffer underfill 203 | 204 | 205 | def serialize_numpy(self, buff, numpy): 206 | """ 207 | serialize message with numpy array types into buffer 208 | :param buff: buffer, ``StringIO`` 209 | :param numpy: numpy python module 210 | """ 211 | try: 212 | pass 213 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 214 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 215 | 216 | def deserialize_numpy(self, str, numpy): 217 | """ 218 | unpack serialized message in str into this message instance using numpy for array types 219 | :param str: byte array of serialized message, ``str`` 220 | :param numpy: numpy python module 221 | """ 222 | if python3: 223 | codecs.lookup_error("rosmsg").msg_type = self._type 224 | try: 225 | end = 0 226 | return self 227 | except struct.error as e: 228 | raise genpy.DeserializationError(e) # most likely buffer underfill 229 | 230 | _struct_I = genpy.struct_I 231 | def _get_struct_I(): 232 | global _struct_I 233 | return _struct_I 234 | class StartTrajectory(object): 235 | _type = 'hello_vscode/StartTrajectory' 236 | _md5sum = 'e97a7b67fc7fb1e97c2ebdff30f93234' 237 | _request_class = StartTrajectoryRequest 238 | _response_class = StartTrajectoryResponse 239 | -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/_Takeoff.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from hello_vscode/TakeoffRequest.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | import genpy 10 | 11 | class TakeoffRequest(genpy.Message): 12 | _md5sum = "351d0c9324942af86fe17feac37b587b" 13 | _type = "hello_vscode/TakeoffRequest" 14 | _has_header = False # flag to mark the presence of a Header object 15 | _full_text = """uint8 id 16 | uint8 groupMask 17 | float32 height 18 | duration duration 19 | """ 20 | __slots__ = ['id','groupMask','height','duration'] 21 | _slot_types = ['uint8','uint8','float32','duration'] 22 | 23 | def __init__(self, *args, **kwds): 24 | """ 25 | Constructor. Any message fields that are implicitly/explicitly 26 | set to None will be assigned a default value. The recommend 27 | use is keyword arguments as this is more robust to future message 28 | changes. You cannot mix in-order arguments and keyword arguments. 29 | 30 | The available fields are: 31 | id,groupMask,height,duration 32 | 33 | :param args: complete set of field values, in .msg order 34 | :param kwds: use keyword arguments corresponding to message field names 35 | to set specific fields. 36 | """ 37 | if args or kwds: 38 | super(TakeoffRequest, self).__init__(*args, **kwds) 39 | # message fields cannot be None, assign default values for those that are 40 | if self.id is None: 41 | self.id = 0 42 | if self.groupMask is None: 43 | self.groupMask = 0 44 | if self.height is None: 45 | self.height = 0. 46 | if self.duration is None: 47 | self.duration = genpy.Duration() 48 | else: 49 | self.id = 0 50 | self.groupMask = 0 51 | self.height = 0. 52 | self.duration = genpy.Duration() 53 | 54 | def _get_types(self): 55 | """ 56 | internal API method 57 | """ 58 | return self._slot_types 59 | 60 | def serialize(self, buff): 61 | """ 62 | serialize message into buffer 63 | :param buff: buffer, ``StringIO`` 64 | """ 65 | try: 66 | _x = self 67 | buff.write(_get_struct_2Bf2i().pack(_x.id, _x.groupMask, _x.height, _x.duration.secs, _x.duration.nsecs)) 68 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 69 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 70 | 71 | def deserialize(self, str): 72 | """ 73 | unpack serialized message in str into this message instance 74 | :param str: byte array of serialized message, ``str`` 75 | """ 76 | if python3: 77 | codecs.lookup_error("rosmsg").msg_type = self._type 78 | try: 79 | if self.duration is None: 80 | self.duration = genpy.Duration() 81 | end = 0 82 | _x = self 83 | start = end 84 | end += 14 85 | (_x.id, _x.groupMask, _x.height, _x.duration.secs, _x.duration.nsecs,) = _get_struct_2Bf2i().unpack(str[start:end]) 86 | self.duration.canon() 87 | return self 88 | except struct.error as e: 89 | raise genpy.DeserializationError(e) # most likely buffer underfill 90 | 91 | 92 | def serialize_numpy(self, buff, numpy): 93 | """ 94 | serialize message with numpy array types into buffer 95 | :param buff: buffer, ``StringIO`` 96 | :param numpy: numpy python module 97 | """ 98 | try: 99 | _x = self 100 | buff.write(_get_struct_2Bf2i().pack(_x.id, _x.groupMask, _x.height, _x.duration.secs, _x.duration.nsecs)) 101 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 102 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 103 | 104 | def deserialize_numpy(self, str, numpy): 105 | """ 106 | unpack serialized message in str into this message instance using numpy for array types 107 | :param str: byte array of serialized message, ``str`` 108 | :param numpy: numpy python module 109 | """ 110 | if python3: 111 | codecs.lookup_error("rosmsg").msg_type = self._type 112 | try: 113 | if self.duration is None: 114 | self.duration = genpy.Duration() 115 | end = 0 116 | _x = self 117 | start = end 118 | end += 14 119 | (_x.id, _x.groupMask, _x.height, _x.duration.secs, _x.duration.nsecs,) = _get_struct_2Bf2i().unpack(str[start:end]) 120 | self.duration.canon() 121 | return self 122 | except struct.error as e: 123 | raise genpy.DeserializationError(e) # most likely buffer underfill 124 | 125 | _struct_I = genpy.struct_I 126 | def _get_struct_I(): 127 | global _struct_I 128 | return _struct_I 129 | _struct_2Bf2i = None 130 | def _get_struct_2Bf2i(): 131 | global _struct_2Bf2i 132 | if _struct_2Bf2i is None: 133 | _struct_2Bf2i = struct.Struct("<2Bf2i") 134 | return _struct_2Bf2i 135 | # This Python file uses the following encoding: utf-8 136 | """autogenerated by genpy from hello_vscode/TakeoffResponse.msg. Do not edit.""" 137 | import codecs 138 | import sys 139 | python3 = True if sys.hexversion > 0x03000000 else False 140 | import genpy 141 | import struct 142 | 143 | 144 | class TakeoffResponse(genpy.Message): 145 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 146 | _type = "hello_vscode/TakeoffResponse" 147 | _has_header = False # flag to mark the presence of a Header object 148 | _full_text = """""" 149 | __slots__ = [] 150 | _slot_types = [] 151 | 152 | def __init__(self, *args, **kwds): 153 | """ 154 | Constructor. Any message fields that are implicitly/explicitly 155 | set to None will be assigned a default value. The recommend 156 | use is keyword arguments as this is more robust to future message 157 | changes. You cannot mix in-order arguments and keyword arguments. 158 | 159 | The available fields are: 160 | 161 | 162 | :param args: complete set of field values, in .msg order 163 | :param kwds: use keyword arguments corresponding to message field names 164 | to set specific fields. 165 | """ 166 | if args or kwds: 167 | super(TakeoffResponse, self).__init__(*args, **kwds) 168 | 169 | def _get_types(self): 170 | """ 171 | internal API method 172 | """ 173 | return self._slot_types 174 | 175 | def serialize(self, buff): 176 | """ 177 | serialize message into buffer 178 | :param buff: buffer, ``StringIO`` 179 | """ 180 | try: 181 | pass 182 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 183 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 184 | 185 | def deserialize(self, str): 186 | """ 187 | unpack serialized message in str into this message instance 188 | :param str: byte array of serialized message, ``str`` 189 | """ 190 | if python3: 191 | codecs.lookup_error("rosmsg").msg_type = self._type 192 | try: 193 | end = 0 194 | return self 195 | except struct.error as e: 196 | raise genpy.DeserializationError(e) # most likely buffer underfill 197 | 198 | 199 | def serialize_numpy(self, buff, numpy): 200 | """ 201 | serialize message with numpy array types into buffer 202 | :param buff: buffer, ``StringIO`` 203 | :param numpy: numpy python module 204 | """ 205 | try: 206 | pass 207 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 208 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 209 | 210 | def deserialize_numpy(self, str, numpy): 211 | """ 212 | unpack serialized message in str into this message instance using numpy for array types 213 | :param str: byte array of serialized message, ``str`` 214 | :param numpy: numpy python module 215 | """ 216 | if python3: 217 | codecs.lookup_error("rosmsg").msg_type = self._type 218 | try: 219 | end = 0 220 | return self 221 | except struct.error as e: 222 | raise genpy.DeserializationError(e) # most likely buffer underfill 223 | 224 | _struct_I = genpy.struct_I 225 | def _get_struct_I(): 226 | global _struct_I 227 | return _struct_I 228 | class Takeoff(object): 229 | _type = 'hello_vscode/Takeoff' 230 | _md5sum = '351d0c9324942af86fe17feac37b587b' 231 | _request_class = TakeoffRequest 232 | _response_class = TakeoffResponse 233 | -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__init__.py: -------------------------------------------------------------------------------- 1 | from ._AddInts import * 2 | from ._Land import * 3 | from ._Takeoff import * 4 | from ._GoTo import * 5 | from ._UploadTrajectory import * 6 | from ._StartTrajectory import * 7 | -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_AddInts.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_AddInts.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_GoTo.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_GoTo.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_Land.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_Land.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_StartTrajectory.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_StartTrajectory.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_Takeoff.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_Takeoff.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_UploadTrajectory.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/_UploadTrajectory.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/pycrazyswarm/srv/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from Cython.Build import cythonize 3 | 4 | setup(ext_modules=cythonize(["_GoTo.py","_Land.py","_StartTrajectory.py", "_Takeoff.py", "_UploadTrajectory.py"])) -------------------------------------------------------------------------------- /src/swarm_brain/scripts/crazyswarm/uav_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | from rospy import Duration 5 | 6 | def normalize(v): 7 | norm = np.linalg.norm(v) 8 | assert norm > 0 9 | return v / norm 10 | 11 | 12 | class Polynomial: 13 | def __init__(self, p): 14 | self.p = p 15 | 16 | def __len__(self): 17 | return len(self.p) 18 | 19 | def __iter__(self): 20 | return iter(self.p) 21 | 22 | # evaluate a polynomial using horner's rule 23 | def eval(self, t): 24 | assert t >= 0 25 | x = 0.0 26 | for i in range(0, len(self.p)): 27 | x = x * t + self.p[len(self.p) - 1 - i] 28 | return x 29 | 30 | # compute and return derivative 31 | def derivative(self): 32 | return Polynomial([(i+1) * self.p[i+1] for i in range(0, len(self.p) - 1)]) 33 | 34 | 35 | class TrajectoryOutput: 36 | def __init__(self): 37 | self.pos = None # position [m] 38 | self.vel = None # velocity [m/s] 39 | self.acc = None # acceleration [m/s^2] 40 | self.omega = None # angular velocity [rad/s] 41 | self.yaw = None # yaw angle [rad] 42 | 43 | 44 | # 4d single polynomial piece for x-y-z-yaw, includes duration. 45 | class Polynomial4D: 46 | def __init__(self, duration, px, py, pz, pyaw): 47 | self.duration = duration 48 | self.px = Polynomial(px) 49 | self.py = Polynomial(py) 50 | self.pz = Polynomial(pz) 51 | self.pyaw = Polynomial(pyaw) 52 | 53 | # compute and return derivative 54 | def derivative(self): 55 | return Polynomial4D( 56 | self.duration, 57 | self.px.derivative().p, 58 | self.py.derivative().p, 59 | self.pz.derivative().p, 60 | self.pyaw.derivative().p) 61 | 62 | def eval(self, t): 63 | result = TrajectoryOutput() 64 | # flat variables 65 | result.pos = np.array([self.px.eval(t), self.py.eval(t), self.pz.eval(t)]) 66 | result.yaw = self.pyaw.eval(t) 67 | 68 | # 1st derivative 69 | derivative = self.derivative() 70 | result.vel = np.array([derivative.px.eval(t), derivative.py.eval(t), derivative.pz.eval(t)]) 71 | dyaw = derivative.pyaw.eval(t) 72 | 73 | # 2nd derivative 74 | derivative2 = derivative.derivative() 75 | result.acc = np.array([derivative2.px.eval(t), derivative2.py.eval(t), derivative2.pz.eval(t)]) 76 | 77 | # 3rd derivative 78 | derivative3 = derivative2.derivative() 79 | jerk = np.array([derivative3.px.eval(t), derivative3.py.eval(t), derivative3.pz.eval(t)]) 80 | 81 | thrust = result.acc + np.array([0, 0, 9.81]) # add gravity 82 | 83 | z_body = normalize(thrust) 84 | x_world = np.array([np.cos(result.yaw), np.sin(result.yaw), 0]) 85 | y_body = normalize(np.cross(z_body, x_world)) 86 | x_body = np.cross(y_body, z_body) 87 | 88 | jerk_orth_zbody = jerk - (np.dot(jerk, z_body) * z_body) 89 | h_w = jerk_orth_zbody / np.linalg.norm(thrust) 90 | 91 | result.omega = np.array([-np.dot(h_w, y_body), np.dot(h_w, x_body), z_body[2] * dyaw]) 92 | return result 93 | 94 | 95 | class Trajectory: 96 | def __init__(self): 97 | self.polynomials = None 98 | self.duration = None 99 | 100 | def n_pieces(self): 101 | return len(self.polynomials) 102 | 103 | def loadcsv(self, filename): 104 | data = np.loadtxt(filename, delimiter=",", skiprows=1, usecols=range(33)) 105 | self.polynomials = [Polynomial4D(row[0], row[1:9], row[9:17], row[17:25], row[25:33]) for row in data] 106 | self.duration = np.sum(data[:,0]) 107 | 108 | def eval(self, t): 109 | assert t >= 0 110 | assert t <= self.duration 111 | 112 | current_t = 0.0 113 | for p in self.polynomials: 114 | if t <= current_t + p.duration: 115 | return p.eval(t - current_t) 116 | current_t = current_t + p.duration 117 | -------------------------------------------------------------------------------- /src/swarm_brain/scripts/wheelswarm/swarm_brain.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from os.path import abspath, join, dirname 3 | sys.path.insert(0, join(abspath(dirname(__file__)))) 4 | 5 | 6 | from utilities.transformations import * 7 | from utilities.barrier_certificates import * 8 | from utilities.misc import * 9 | from utilities.controllers import * 10 | 11 | import numpy as np 12 | import rospy 13 | from threading import Thread 14 | from geometry_msgs.msg import Twist 15 | from nav_msgs.msg import Odometry 16 | import re 17 | import tf 18 | 19 | 20 | # 获取无人车数量 21 | kkswarm_num = rospy.get_param('num_robot') 22 | 23 | kkswarm_state = dict() 24 | kkswarm_publishers = dict() 25 | 26 | # 定义无人车集群publishers 27 | for id in range(1, kkswarm_num+1): 28 | topic_cmd_velocity = "/robot_" + str(id) + "/cmd_vel" 29 | pub = rospy.Publisher(topic_cmd_velocity, Twist, queue_size=10) 30 | cmd_vel_msg = Twist() 31 | kkswarm_publishers[id] = [pub, cmd_vel_msg] 32 | 33 | 34 | # 订阅无人车状态回调汉书 35 | def subPose_CallBack(data): 36 | agent_id = int(re.findall(r'\d+', data.header.frame_id)[0]) 37 | kkswarm_state[agent_id] = data 38 | 39 | # 设置无人车集群subscriber 40 | def handler1(): 41 | for id in range(1,kkswarm_num+1): 42 | topic_pos = "/robot_" + str(id) + "/pose" 43 | rospy.Subscriber(topic_pos, Odometry, subPose_CallBack) 44 | 45 | rospy.spin() 46 | 47 | # 无人车集群控制核心代码 48 | def handler2(): 49 | si_barrier_cert = cbf_single_integrator(barrier_gain=100, safety_radius=0.3, magnitude_limit=0.35) 50 | 51 | # 创建单积分位置控制器 52 | si_position_controller = create_si_position_controller() 53 | 54 | # 创建单积分与独轮车模型转换函数 55 | si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping() 56 | 57 | # 设置控制频率25Hz 58 | rate = rospy.Rate(50) 59 | 60 | # 设置算法相关参数 循环次数 内外半径 边界 交换的位置点 61 | num_cycles=10 62 | count = -1 63 | flag = 0 64 | 65 | radius = 0.8 66 | radius2 = 1.8 67 | xybound = radius*np.array([-1, 1, -1, 1]) 68 | p_theta = 2*np.pi*(np.arange(0, 2*kkswarm_num/2, 2)/(2*kkswarm_num/2)) 69 | p_circ = np.vstack([ 70 | np.hstack([xybound[1]*np.cos(p_theta), xybound[1]*np.cos(p_theta+np.pi)]), 71 | np.hstack([xybound[3]*np.sin(p_theta), xybound[3]*np.sin(p_theta+np.pi)]) 72 | ]) 73 | 74 | xybound2 = radius2*np.array([-1, 1, -1, 1]) 75 | p_theta2 = 2*np.pi*(np.arange(0, 2*int(kkswarm_num/2), 2)/(2*int(kkswarm_num/2))) 76 | p_circ2 = np.vstack([ 77 | np.hstack([xybound2[1]*np.cos(p_theta2), xybound2[1]*np.cos(p_theta2+np.pi)]), 78 | np.hstack([xybound2[3]*np.sin(p_theta2), xybound2[3]*np.sin(p_theta2+np.pi)]) 79 | ]) 80 | x_goal = p_circ[:, :int(kkswarm_num/2)] 81 | x_goal2 = p_circ2[:, :int(kkswarm_num/2)] 82 | x_goals = np.concatenate((x_goal, x_goal2), axis=1) 83 | 84 | kkswarm_poses = np.zeros((3, kkswarm_num)) 85 | 86 | # 主循环 87 | while not rospy.is_shutdown(): 88 | if len(kkswarm_state) < kkswarm_num: 89 | continue 90 | for agent_id in range(1,kkswarm_num+1): 91 | odom = kkswarm_state[agent_id] 92 | kkswarm_poses[0, agent_id - 1] = odom.pose.pose.position.x 93 | kkswarm_poses[1, agent_id - 1] = odom.pose.pose.position.y 94 | orientation = (odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w) 95 | euler = tf.transformations.euler_from_quaternion(orientation) 96 | kkswarm_poses[2, agent_id - 1] = euler[2] 97 | 98 | x_si = uni_to_si_states(kkswarm_poses) 99 | 100 | # 判断是否每个无人车是否到达目标点 101 | if(np.linalg.norm(x_goals - x_si) < 0.05): 102 | flag = 1-flag 103 | count += 1 104 | 105 | # 是否达到最大循环运行次数 106 | if count == num_cycles: 107 | break 108 | 109 | # 判断是否改变交换的位置点 110 | if(flag == 0): 111 | x_goal = p_circ[:, :int(kkswarm_num/2)] 112 | x_goal2 = p_circ2[:, :int(kkswarm_num/2)] 113 | else: 114 | x_goal = p_circ[:, int(kkswarm_num/2):] 115 | x_goal2 = p_circ2[:, int(kkswarm_num/2):] 116 | x_goals = np.concatenate((x_goal, x_goal2), axis=1) 117 | 118 | # 根据位置和目标点计算速度矢量 119 | dxi = si_position_controller(x_si,x_goals) 120 | 121 | # 考虑避障情况 122 | dxi = si_barrier_cert(dxi, x_si) 123 | 124 | # 将单积分转换线速度和角速度 125 | dxu = si_to_uni_dyn(dxi, kkswarm_poses) 126 | 127 | # 发布无人车控制指令 128 | for agent_id in range(1, kkswarm_num + 1): 129 | pub,msg = kkswarm_publishers[agent_id] 130 | msg.linear.x = dxu[0, agent_id - 1] 131 | msg.angular.z = dxu[1, agent_id - 1] 132 | pub.publish(msg) 133 | 134 | 135 | rate.sleep() 136 | 137 | def main(): 138 | # 初始化ROS节点 139 | rospy.init_node("swarm_brain") 140 | 141 | # 启动多线程 142 | t1 = Thread(target=handler1, args=()) 143 | t2 = Thread(target=handler2, args=()) 144 | 145 | t1.start() 146 | t2.start() 147 | 148 | if __name__ == "__main__": 149 | main() 150 | 151 | 152 | 153 | -------------------------------------------------------------------------------- /src/swarm_brain/scripts/wheelswarm/utilities/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/wheelswarm/utilities/__init__.py -------------------------------------------------------------------------------- /src/swarm_brain/scripts/wheelswarm/utilities/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/wheelswarm/utilities/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_brain/scripts/wheelswarm/utilities/barrier_certificates.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/wheelswarm/utilities/barrier_certificates.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_brain/scripts/wheelswarm/utilities/controllers.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/wheelswarm/utilities/controllers.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_brain/scripts/wheelswarm/utilities/graph.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/wheelswarm/utilities/graph.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_brain/scripts/wheelswarm/utilities/misc.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/wheelswarm/utilities/misc.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_brain/scripts/wheelswarm/utilities/transformations.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_brain/scripts/wheelswarm/utilities/transformations.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_view/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(swarm_view) 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 | geometry_msgs 15 | message_generation 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 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | generate_messages( 74 | DEPENDENCIES 75 | std_msgs 76 | ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES swarm_view 110 | CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs message_runtime 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | # include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Declare a C++ library 126 | # add_library(${PROJECT_NAME} 127 | # src/${PROJECT_NAME}/swarm_view.cpp 128 | # ) 129 | 130 | ## Add cmake target dependencies of the library 131 | ## as an example, code may need to be generated before libraries 132 | ## either from message generation or dynamic reconfigure 133 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 134 | 135 | ## Declare a C++ executable 136 | ## With catkin_make all packages are built within a single CMake context 137 | ## The recommended prefix ensures that target names across packages don't collide 138 | # add_executable(${PROJECT_NAME}_node src/swarm_view_node.cpp) 139 | 140 | ## Rename C++ executable without prefix 141 | ## The above recommended prefix causes long target names, the following renames the 142 | ## target back to the shorter version for ease of user use 143 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 144 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(${PROJECT_NAME}_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | catkin_install_python(PROGRAMS 165 | scripts/pybullet_crazyswarm.py 166 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | ) 168 | 169 | ## Mark executables for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 171 | # install(TARGETS ${PROJECT_NAME}_node 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark libraries for installation 176 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 177 | # install(TARGETS ${PROJECT_NAME} 178 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 181 | # ) 182 | 183 | ## Mark cpp header files for installation 184 | # install(DIRECTORY include/${PROJECT_NAME}/ 185 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 186 | # FILES_MATCHING PATTERN "*.h" 187 | # PATTERN ".svn" EXCLUDE 188 | # ) 189 | 190 | ## Mark other files for installation (e.g. launch and bag files, etc.) 191 | # install(FILES 192 | # # myfile1 193 | # # myfile2 194 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 195 | # ) 196 | 197 | ############# 198 | ## Testing ## 199 | ############# 200 | 201 | ## Add gtest based cpp test target and link libraries 202 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_swarm_view.cpp) 203 | # if(TARGET ${PROJECT_NAME}-test) 204 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 205 | # endif() 206 | 207 | ## Add folders to be run by python nosetests 208 | # catkin_add_nosetests(test) 209 | -------------------------------------------------------------------------------- /src/swarm_view/launch/start_pybullet4crazyswarm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /src/swarm_view/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | swarm_view 4 | 0.0.0 5 | The swarm_view package 6 | 7 | 8 | 9 | 10 | amov 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | geometry_msgs 59 | message_generation 60 | roscpp 61 | rospy 62 | std_msgs 63 | geometry_msgs 64 | message_runtime 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /src/swarm_view/scripts/assets/architrave.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /src/swarm_view/scripts/assets/cf2p.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 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 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /src/swarm_view/scripts/assets/cf2x.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 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 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /src/swarm_view/scripts/pybullet_crazyswarm.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from os.path import abspath, join, dirname 3 | sys.path.insert(0, join(abspath(dirname(__file__)))) 4 | print(abspath(dirname(__file__))) 5 | 6 | import pybullet as p 7 | import rospy 8 | from nav_msgs.msg import Odometry 9 | from pycrazyswarm.msg import * 10 | import pybullet_data 11 | import re 12 | from threading import Thread 13 | import random 14 | import numpy as np 15 | import tf 16 | 17 | def robot_pose_callback(msg): 18 | agent_id = int(re.findall(r'\d+', msg.header.frame_id)[0]) 19 | new_pos = [msg.pose.pose.position.x, msg.pose.pose.position.y, 0.01] 20 | orientation = (msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w) 21 | euler = tf.transformations.euler_from_quaternion(orientation) 22 | # new_orien= [msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w] 23 | 24 | p.resetBasePositionAndOrientation(robotIds[agent_id-1], new_pos, p.getQuaternionFromEuler([1.57, euler[1], euler[2]])) 25 | 26 | def drone_pose_callback(msg): 27 | cf_id = int(re.findall(r'\d+', msg.header.frame_id)[0]) 28 | new_pos = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z] 29 | new_orien= [msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z,msg.pose.orientation.w] 30 | 31 | trajectory_dict[msg.header.frame_id].append(new_pos) 32 | p.resetBasePositionAndOrientation(droneIds[cf_id-1], new_pos, new_orien) 33 | 34 | 35 | 36 | def handler1(): 37 | rate = rospy.Rate(50) 38 | # 循环更新并绘制无人机位置 39 | while not rospy.is_shutdown(): 40 | # 更新物理仿真 41 | p.stepSimulation() 42 | p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING) 43 | rate.sleep() 44 | 45 | 46 | def handler2(): 47 | yaw = 0.0 48 | pitch = -45.0 49 | camera_target_pos = [0, 0, 0] 50 | 51 | # 循环运行pybullet引擎 52 | while True: 53 | # 从键盘读取按键输入 54 | keys = p.getKeyboardEvents() 55 | 56 | # 根据按键输入更新相机位置和目标位置 57 | for key, state in keys.items(): 58 | if key == p.B3G_UP_ARROW and (state & p.KEY_WAS_TRIGGERED): 59 | pitch -= 5.0 60 | elif key == p.B3G_DOWN_ARROW and (state & p.KEY_WAS_TRIGGERED): 61 | pitch += 5.0 62 | elif key == p.B3G_LEFT_ARROW and (state & p.KEY_WAS_TRIGGERED): 63 | yaw -= 5.0 64 | elif key == p.B3G_RIGHT_ARROW and (state & p.KEY_WAS_TRIGGERED): 65 | yaw += 5.0 66 | 67 | # 更新相机位置和目标位置 68 | p.resetDebugVisualizerCamera(cameraDistance=5, cameraYaw=yaw, cameraPitch=pitch, 69 | cameraTargetPosition=camera_target_pos) 70 | 71 | def handler3(): 72 | 73 | line_width = 3 74 | 75 | while True: 76 | # 获取键盘事件 77 | events = p.getKeyboardEvents() 78 | 79 | # 检查是否按下了 't' 键 80 | if ord('t') in events: 81 | # 执行相应的操作 82 | print("键盘按下了 't'。") 83 | 84 | 85 | for key, traj in trajectory_dict.items(): 86 | print(key, traj) 87 | line_color = [random.random(), random.random(), random.random()] 88 | for i in range(len(traj) - 1): 89 | line_start_point = traj[i] 90 | line_end_point = traj[i + 1] 91 | p.addUserDebugLine( 92 | lineFromXYZ=line_start_point, lineToXYZ=line_end_point, 93 | lineColorRGB=line_color, lineWidth=line_width) 94 | 95 | break 96 | 97 | 98 | 99 | 100 | if __name__ == "__main__": 101 | rospy.init_node('pybullet_drone_display') 102 | 103 | # 设置物理仿真环境 104 | physicsClient = p.connect(p.GUI) 105 | p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) 106 | p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) 107 | 108 | p.setGravity(0, 0, 0) 109 | p.setAdditionalSearchPath(pybullet_data.getDataPath()) # 设置pybullet数据路径 110 | planeId = p.loadURDF("plane.urdf") 111 | p.setRealTimeSimulation(True) 112 | 113 | trajectory_dict = dict() 114 | 115 | quad_num = len(rospy.get_param('crazyflies')) 116 | 117 | 118 | 119 | # 添加无人机模型 120 | droneIds = [] 121 | for id in range(quad_num): 122 | # pybullet init 123 | drone_urdf = dirname(__file__) + "/assets/cf2p.urdf" 124 | droneId = p.loadURDF(drone_urdf) 125 | droneIds.append(droneId) 126 | 127 | # ros 128 | topic_pos = "/cf" + str(int(id + 1)) + "/fullstate" 129 | rospy.Subscriber(topic_pos, FullState, drone_pose_callback) 130 | 131 | trajectory_dict["/cf"+str(int(id + 1))] = [] 132 | 133 | t1 = Thread(target=handler1, args=()) 134 | t2 = Thread(target=handler2, args=()) 135 | t3 = Thread(target=handler3, args=()) 136 | 137 | t1.start() 138 | t2.start() 139 | t3.start() 140 | 141 | 142 | 143 | 144 | 145 | 146 | -------------------------------------------------------------------------------- /src/swarm_view/scripts/pycrazyswarm/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_view/scripts/pycrazyswarm/__init__.py -------------------------------------------------------------------------------- /src/swarm_view/scripts/pycrazyswarm/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_view/scripts/pycrazyswarm/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/swarm_view/scripts/pycrazyswarm/msg/_FullState.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_view/scripts/pycrazyswarm/msg/_FullState.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_view/scripts/pycrazyswarm/msg/_GenericLogData.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_view/scripts/pycrazyswarm/msg/_GenericLogData.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_view/scripts/pycrazyswarm/msg/_Position.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_view/scripts/pycrazyswarm/msg/_Position.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_view/scripts/pycrazyswarm/msg/_TrajectoryPolynomialPiece.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_view/scripts/pycrazyswarm/msg/_TrajectoryPolynomialPiece.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_view/scripts/pycrazyswarm/msg/_VelocityWorld.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_view/scripts/pycrazyswarm/msg/_VelocityWorld.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/swarm_view/scripts/pycrazyswarm/msg/__init__.py: -------------------------------------------------------------------------------- 1 | from ._Position import * 2 | from ._TrajectoryPolynomialPiece import * 3 | from ._VelocityWorld import * 4 | from ._FullState import * 5 | from ._GenericLogData import * 6 | -------------------------------------------------------------------------------- /src/swarm_view/scripts/pycrazyswarm/msg/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/swarm_view/scripts/pycrazyswarm/msg/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/wheelswarm_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(wheelswarm_ros) 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 | geometry_msgs 15 | nav_msgs 16 | message_generation 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | generate_messages( 75 | DEPENDENCIES 76 | std_msgs 77 | ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES wheelswarm_ros 111 | CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs nav_msgs message_runtime 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | include_directories( 122 | # include 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/wheelswarm_ros.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/wheelswarm_ros_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # file(GLOB PYTHON_SCRIPTS scripts/rps/*.py) 166 | catkin_install_python(PROGRAMS scripts/rps/wheelswarm_sim.py 167 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | ) 169 | 170 | ## Mark executables for installation 171 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 172 | # install(TARGETS ${PROJECT_NAME}_node 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark libraries for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 178 | # install(TARGETS ${PROJECT_NAME} 179 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark cpp header files for installation 185 | # install(DIRECTORY include/${PROJECT_NAME}/ 186 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 187 | # FILES_MATCHING PATTERN "*.h" 188 | # PATTERN ".svn" EXCLUDE 189 | # ) 190 | 191 | ## Mark other files for installation (e.g. launch and bag files, etc.) 192 | # install(FILES 193 | # # myfile1 194 | # # myfile2 195 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | # ) 197 | 198 | ############# 199 | ## Testing ## 200 | ############# 201 | 202 | ## Add gtest based cpp test target and link libraries 203 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_wheelswarm_ros.cpp) 204 | # if(TARGET ${PROJECT_NAME}-test) 205 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 206 | # endif() 207 | 208 | ## Add folders to be run by python nosetests 209 | # catkin_add_nosetests(test) 210 | -------------------------------------------------------------------------------- /src/wheelswarm_ros/config/wheelswarm.yaml: -------------------------------------------------------------------------------- 1 | num_robot: 12 -------------------------------------------------------------------------------- /src/wheelswarm_ros/launch/start_wheelswarm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/wheelswarm_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | wheelswarm_ros 4 | 0.0.0 5 | The wheelswarm_ros package 6 | 7 | 8 | 9 | 10 | amov 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | geometry_msgs 56 | nav_msgs 57 | message_generation 58 | roscpp 59 | rospy 60 | std_msgs 61 | roscpp 62 | rospy 63 | std_msgs 64 | geometry_msgs 65 | nav_msgs 66 | message_runtime 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /src/wheelswarm_ros/scripts/rps/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/wheelswarm_ros/scripts/rps/__init__.py -------------------------------------------------------------------------------- /src/wheelswarm_ros/scripts/rps/__version__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/wheelswarm_ros/scripts/rps/__version__.py -------------------------------------------------------------------------------- /src/wheelswarm_ros/scripts/rps/kkrobot.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/wheelswarm_ros/scripts/rps/kkrobot.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/wheelswarm_ros/scripts/rps/kkrobot_abc.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/wheelswarm_ros/scripts/rps/kkrobot_abc.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/wheelswarm_ros/scripts/rps/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from Cython.Build import cythonize 3 | 4 | setup(ext_modules=cythonize(["kkrobot_abc.py"])) 5 | -------------------------------------------------------------------------------- /src/wheelswarm_ros/scripts/rps/utilities/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/wheelswarm_ros/scripts/rps/utilities/__init__.py -------------------------------------------------------------------------------- /src/wheelswarm_ros/scripts/rps/utilities/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/wheelswarm_ros/scripts/rps/utilities/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/wheelswarm_ros/scripts/rps/utilities/misc.cpython-310-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AISwarmLab/swarmSim/f937701a98c53a0b438113e79c5a27b5f3f121d6/src/wheelswarm_ros/scripts/rps/utilities/misc.cpython-310-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /src/wheelswarm_ros/scripts/rps/wheelswarm_sim.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from os.path import abspath, join, dirname 3 | sys.path.insert(0, join(abspath(dirname(__file__)))) 4 | 5 | import kkrobot 6 | import numpy as np 7 | import rospy 8 | import random 9 | import math 10 | 11 | def main(): 12 | # 获取无人车的数量 13 | N = rospy.get_param('num_robot') 14 | 15 | # 初始化ROS节点 16 | rospy.init_node("kkswarm_simulator", anonymous=True) 17 | 18 | # 自定义初始位置 19 | init_poses = np.zeros((3,N)) 20 | 21 | for i in range(N): 22 | if i < int(N/2): 23 | radius = random.uniform(0, 1) 24 | theta = random.uniform(0, 2*math.pi) 25 | init_poses[0, i] = radius*math.cos(theta) 26 | init_poses[1, i] = radius*math.sin(theta) 27 | init_poses[2, i] = np.random.rand()*2*np.pi - np.pi 28 | else: 29 | break 30 | 31 | i = int(N/2) 32 | while i < N: 33 | radius = random.uniform(1, 2) 34 | theta = random.uniform(0, 2*math.pi) 35 | init_poses[0, i] = radius*math.cos(theta) 36 | init_poses[1, i] = radius*math.sin(theta) 37 | 38 | if (init_poses[0, i] < 1.0 and init_poses[0, i]> -1.0) or (init_poses[1, i] < 1.0 and init_poses[1, i]> -1.0): 39 | continue 40 | else: 41 | init_poses[2, i] = np.random.rand()*2*np.pi - np.pi 42 | i+=1 43 | 44 | 45 | # 初始化KKSwarm仿真器引擎 46 | r = kkrobot.KKRobot(number_of_robots=N, show_figure=True, sim_in_real_time=True, initial_conditions = init_poses) 47 | rate = rospy.Rate(1.0 / r.time_step) 48 | 49 | # 启动KKSwarm仿真器引擎 50 | while not rospy.is_shutdown(): 51 | r.step() 52 | rate.sleep() 53 | 54 | if __name__ == "__main__": 55 | main() 56 | 57 | 58 | 59 | --------------------------------------------------------------------------------