├── README.md ├── icp ├── Document.md ├── course_agv_icp │ ├── .DS_Store │ ├── .catkin_tools │ │ ├── CATKIN_IGNORE │ │ ├── README │ │ ├── VERSION │ │ └── profiles │ │ │ └── default │ │ │ ├── build.yaml │ │ │ ├── devel_collisions.txt │ │ │ └── packages │ │ │ └── catkin_tools_prebuild │ │ │ ├── devel_manifest.txt │ │ │ └── package.xml │ ├── .gitignore │ ├── CMakeLists.txt │ ├── launch │ │ ├── course_agv.rviz │ │ └── icp.launch │ ├── package.xml │ └── src │ │ ├── .DS_Store │ │ ├── icp.cpp │ │ └── tf.cpp └── images │ ├── 1.png │ ├── 2.png │ ├── 2020-08-11 15-01-05屏幕截图.png │ ├── 2020-08-12 23-21-35屏幕截图.png │ ├── 2020-08-15 11-41-24屏幕截图.png │ ├── 2020-08-17 11-13-15屏幕截图.png │ ├── 2020-08-18 22-34-54屏幕截图.png │ ├── 2020-08-19 10-57-05屏幕截图.png │ ├── 2020-08-19 14-58-22屏幕截图.png │ ├── 2020-08-19 14-58-48屏幕截图.png │ ├── 2020-08-19 14-59-16屏幕截图.png │ ├── 2020-08-19 15-04-03屏幕截图.png │ ├── 2020-08-19 15-09-58屏幕截图.png │ ├── 2020-08-19 15-12-31屏幕截图.png │ ├── 2020-08-19 15-12-47屏幕截图.png │ ├── 2020-08-19 15-15-48屏幕截图.png │ ├── 2020-08-21 10-23-13屏幕截图.png │ ├── 2020-08-21 10-25-01屏幕截图.png │ ├── 2020-08-21 10-30-08屏幕截图.png │ ├── 2020-08-21 10-30-19屏幕截图.png │ ├── 2020-08-21 10-32-22屏幕截图.png │ ├── 2020-08-21 10-32-37屏幕截图.png │ ├── 2020-08-21 10-35-12屏幕截图.png │ ├── 2020-08-21 10-36-23屏幕截图.png │ ├── 2020-08-21 10-37-23屏幕截图.png │ ├── 2020-08-21 10-40-06屏幕截图.png │ ├── 2020-08-21 13-57-03屏幕截图.png │ ├── 2020-08-21 13-57-22屏幕截图.png │ ├── 2020-08-21 14-01-05屏幕截图.png │ ├── 2020-08-21 14-04-46屏幕截图.png │ ├── 2020-08-23 21-23-14屏幕截图.png │ ├── 2020-08-23 22-05-52屏幕截图.png │ ├── 2020-08-24 20-45-05屏幕截图.png │ ├── 2020-08-24 21-05-44屏幕截图.png │ ├── 2020-08-24 21-06-18屏幕截图.png │ ├── 2020-08-24 21-17-47屏幕截图.png │ ├── 3.png │ ├── 4.png │ ├── after shrink.png │ ├── before shrink.png │ ├── icp0.png │ ├── icp00.png │ ├── icp1.png │ ├── icp2.png │ ├── icp3.png │ ├── icp4.png │ └── icp5.png ├── robot_arm ├── Document.md ├── Document.pdf ├── images │ ├── 2020-08-11 15-01-05屏幕截图.png │ ├── 2020-08-12 23-21-35屏幕截图.png │ ├── 2020-08-15 11-41-24屏幕截图.png │ ├── 2020-08-17 11-13-15屏幕截图.png │ ├── 2020-08-18 22-34-54屏幕截图.png │ ├── 2020-08-19 10-57-05屏幕截图.png │ ├── 2020-08-19 14-58-22屏幕截图.png │ ├── 2020-08-19 14-58-48屏幕截图.png │ ├── 2020-08-19 14-59-16屏幕截图.png │ ├── 2020-08-19 15-04-03屏幕截图.png │ ├── 2020-08-19 15-09-58屏幕截图.png │ ├── 2020-08-19 15-12-31屏幕截图.png │ ├── 2020-08-19 15-12-47屏幕截图.png │ ├── 2020-08-19 15-15-48屏幕截图.png │ ├── 2020-08-21 10-22-28屏幕截图.png │ ├── 2020-08-21 10-23-13屏幕截图.png │ ├── 2020-08-21 10-25-01屏幕截图.png │ ├── 2020-08-21 10-30-08屏幕截图.png │ ├── 2020-08-21 10-30-19屏幕截图.png │ ├── 2020-08-21 10-32-22屏幕截图.png │ ├── 2020-08-21 10-32-37屏幕截图.png │ ├── 2020-08-21 10-35-12屏幕截图.png │ ├── 2020-08-21 10-36-23屏幕截图.png │ ├── 2020-08-21 10-37-23屏幕截图.png │ ├── 2020-08-21 10-40-06屏幕截图.png │ ├── 2020-08-21 13-57-03屏幕截图.png │ ├── 2020-08-21 13-57-22屏幕截图.png │ ├── 2020-08-21 14-01-05屏幕截图.png │ ├── 2020-08-21 14-04-46屏幕截图.png │ ├── README.md │ ├── after shrink.png │ ├── before shrink.png │ └── 点到点轨迹规划.png ├── probot_gazebo │ ├── .idea │ │ ├── misc.xml │ │ ├── modules.xml │ │ ├── probot_gazebo.iml │ │ └── workspace.xml │ ├── CMakeLists.txt │ ├── config │ │ ├── probot_anno_control.yaml │ │ ├── probot_anno_gazebo_joint_states.yaml │ │ ├── probot_anno_position_control.yaml │ │ └── probot_anno_velocity_control.yaml │ ├── include │ │ └── ikfast.h │ ├── launch │ │ ├── probot_anno_position_control │ │ │ ├── planning_context_pos.launch │ │ │ ├── probot_anno_gazebo_states.launch │ │ │ ├── probot_anno_gazebo_world_pos.launch │ │ │ ├── probot_anno_position_control_bringup.launch │ │ │ └── probot_anno_position_controller.launch │ │ ├── probot_anno_velocity_control │ │ │ ├── planning_context_vel.launch │ │ │ ├── probot_anno_gazebo_states.launch │ │ │ ├── probot_anno_gazebo_world_vel.launch │ │ │ ├── probot_anno_velocity_control_bringup.launch │ │ │ └── probot_anno_velocity_controller.launch │ │ └── probot_anno_velocity_control_ring │ │ │ ├── planning_context_vel.launch │ │ │ ├── probot_anno_gazebo_states.launch │ │ │ ├── probot_anno_gazebo_world_vel.launch │ │ │ ├── probot_anno_velocity_control_ring_bringup.launch │ │ │ └── probot_anno_velocity_controller.launch │ ├── package.xml │ ├── src │ │ ├── DK.cpp │ │ ├── DK.m │ │ ├── IK.cpp │ │ ├── IK.m │ │ ├── control_example.cpp │ │ ├── control_robot_vel.cpp │ │ ├── trajectory_planning.cpp │ │ ├── trajwithobstacle.cpp │ │ └── trajwithobstacle_real.cpp │ └── worlds │ │ ├── ring_obstacle.world │ │ ├── tableObj.world │ │ └── tableObj2.world ├── 实验1：正逆运动学 │ ├── DK.cpp │ ├── DK.m │ ├── IK.cpp │ └── IK.m ├── 实验2：速度传递实验 │ └── control_robot_vel.cpp ├── 实验3：轨迹规划实验 │ └── trajectory_planning.cpp ├── 实验4：敲铃实验（仿真） │ └── trajwithobstacle.cpp └── 实验4：敲铃实验（实际） │ └── trajwithobstacle_real.cpp └── stupid_yellow_car ├── Document.md ├── course_agv_nav ├── .DS_Store ├── CMakeLists.txt ├── launch │ ├── nav.launch │ ├── nav.rviz │ └── replan.launch ├── package.xml ├── scripts │ ├── .DS_Store │ ├── dwa.py │ ├── global_planner.py │ ├── local_planner.py │ ├── rrt.py │ └── stupid_tracking.py └── srv │ └── Plan.srv └── images ├── 1.png ├── 2.png ├── 2020-08-11 15-01-05屏幕截图.png ├── 2020-08-12 23-21-35屏幕截图.png ├── 2020-08-15 11-41-24屏幕截图.png ├── 2020-08-17 11-13-15屏幕截图.png ├── 2020-08-18 22-34-54屏幕截图.png ├── 2020-08-19 10-57-05屏幕截图.png ├── 2020-08-19 14-58-22屏幕截图.png ├── 2020-08-19 14-58-48屏幕截图.png ├── 2020-08-19 14-59-16屏幕截图.png ├── 2020-08-19 15-04-03屏幕截图.png ├── 2020-08-19 15-09-58屏幕截图.png ├── 2020-08-19 15-12-31屏幕截图.png ├── 2020-08-19 15-12-47屏幕截图.png ├── 2020-08-19 15-15-48屏幕截图.png ├── 2020-08-21 10-23-13屏幕截图.png ├── 2020-08-21 10-25-01屏幕截图.png ├── 2020-08-21 10-30-08屏幕截图.png ├── 2020-08-21 10-30-19屏幕截图.png ├── 2020-08-21 10-32-22屏幕截图.png ├── 2020-08-21 10-32-37屏幕截图.png ├── 2020-08-21 10-35-12屏幕截图.png ├── 2020-08-21 10-36-23屏幕截图.png ├── 2020-08-21 10-37-23屏幕截图.png ├── 2020-08-21 10-40-06屏幕截图.png ├── 2020-08-21 13-57-03屏幕截图.png ├── 2020-08-21 13-57-22屏幕截图.png ├── 2020-08-21 14-01-05屏幕截图.png ├── 2020-08-21 14-04-46屏幕截图.png ├── 2020-08-23 21-23-14屏幕截图.png ├── 2020-08-23 22-05-52屏幕截图.png ├── 2020-08-24 20-45-05屏幕截图.png ├── 2020-08-24 21-05-44屏幕截图.png ├── 2020-08-24 21-06-18屏幕截图.png ├── 2020-08-24 21-17-47屏幕截图.png ├── after shrink.png └── before shrink.png /README.md: -------------------------------------------------------------------------------- 1 | # Advanced_Robotics 2 | path planning, trajectory planning, icp, robot arm project 3 | 4 | ZJU summer class of Prof.Xiong Rong, Prof.Zhou Chunlin, ZJU Robotics Lab 5 | 6 | 7 | 8 | - **stupid_yellow_car**: path planning and trajectory planning based on the JIAZHI yellow car 9 | 10 | `source file:` /course_agv_nav/scripts/rrt.py dwa.py global_planner.py local_planner.py 11 | 12 | `document for reference:` [Document.md](stupid_yellow_car/Document.md) 13 | 14 | - **icp**: icp project based on rosbag simulation 15 | 16 | `source file:` /course_sgv_icp/src/icp.cpp 17 | 18 | `document for reference:` [Document.md](icp/Document.md) 19 | 20 | - **robot_arm**: robot arm experiments based on the Anno robot arm 21 | - Experiment 1: DK & IK 22 | - Experiment 2: Velocity Passing 23 | - Experiment 3: Trajectory Planning 24 | - Experiment 4: Knocking the Rings (Simulation & Real) 25 | 26 | `source file:` /probot_gazebo/src 27 | 28 | `document for reference:` [Document.md](robot_arm/Document.md) / [Document.pdf](robot_arm/Document.pdf) 29 | 30 | -------------------------------------------------------------------------------- /icp/Document.md: -------------------------------------------------------------------------------- 1 | # 机器人学强化训练 里程估计实践报告 2 | 3 | ## 一、里程估计方法简介 4 | 5 | 由于本次实验中agv小车配备高精度的激光雷达传感器，因此本次里程估计任务我们主要借助激光数据和icp算法实现小车运动的里程估计，并在基本icp算法上做一定改进，借助滤波提高icp算法精准度。积累运动中积攒的激光点云数据，通过变换得到点云地图。 6 | 7 | 8 | 9 | ## 二、程序设计说明 10 | 11 | 程序结构设计： 12 | 13 | ```bash 14 | ├── course_agv_icp 15 | │ ├── CMakeLists.txt 16 | │ ├── launch 17 | │ │ ├── course_agv.rviz 18 | │ │ └── icp.launch 19 | │ ├── package.xml 20 | │ └── scripts 21 | │ ├── icp.cpp 22 | │ ├── tf.cpp 23 | ``` 24 | 25 | icp.cpp中只有一个icp类，下面结合代码对icp类中重要函数进行说明 26 | 27 | ```C++ 28 | class icp{ 29 | 30 | public: 31 | 32 | icp(ros::NodeHandle &n); 33 | ~icp(); 34 | ros::NodeHandle& n; 35 | 36 | // robot init states 37 | double robot_x; 38 | double robot_y; 39 | double robot_theta; 40 | // sensor states = robot_x_y_theta 41 | Vector3d sensor_sta; 42 | 43 | // max iterations 44 | int max_iter; 45 | // distance threshold for filter the matching points 46 | double dis_th; 47 | // tolerance to stop icp 48 | double tolerance; 49 | // if is the first scan, set as the map/target 50 | bool isFirstScan; 51 | // src point cloud matrix 52 | MatrixXd src_pc; 53 | // target point cloud matrix 54 | MatrixXd tar_pc; 55 | 56 | // used for point cloud calc 57 | pcl::PointCloud cloud; 58 | Eigen::Matrix3d PtTransform; 59 | void addVecToCloud(Eigen::MatrixXd moved_pc); 60 | 61 | // ICP process function 62 | void process(sensor_msgs::LaserScan input); 63 | // transform the ros msg to Eigen Matrix 64 | Eigen::MatrixXd rosmsgToEigen(const sensor_msgs::LaserScan input); 65 | // fint the nearest points & filter 66 | NeighBor findNearest(const Eigen::MatrixXd &src, const Eigen::MatrixXd &tar); 67 | // get the transform from two point sets in one iteration 68 | Eigen::Matrix3d getTransform(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B); 69 | // calc 2D Euclidean distance 70 | float calc_dist(const Eigen::Vector2d &pta, const Eigen::Vector2d &ptb); 71 | // transform vector states to matrix form 72 | Eigen::Matrix3d staToMatrix(const Vector3d sta); 73 | 74 | // ros-related subscribers, publishers and broadcasters 75 | ros::Subscriber laser_sub; 76 | void publishResult(Matrix3d T); 77 | tf::TransformBroadcaster odom_broadcaster; 78 | ros::Publisher odom_pub; 79 | }; 80 | ``` 81 | 82 | 83 | 84 | ```C++ 85 | Eigen::MatrixXd rosmsgToEigen(const sensor_msgs::LaserScan input); 86 | ``` 87 | 88 | 将传感器读到的角度与距离数据转换为机器人局部坐标系下的x,y坐标数据 89 | 90 | 91 | 92 | ```C++ 93 | NeighBor findNearest(const Eigen::MatrixXd &src, const Eigen::MatrixXd &tar); 94 | ``` 95 | 96 | 对两帧点云图进行匹配，并实现滤波 97 | 98 | 99 | 100 | ```C++ 101 | float calc_dist(const Eigen::Vector2d &pta, const Eigen::Vector2d &ptb); 102 | ``` 103 | 104 | 计算两点间欧氏距离 105 | 106 | 107 | 108 | ```C++ 109 | Eigen::Matrix3d staToMatrix(const Vector3d sta); 110 | ``` 111 | 112 | 将3 * 1向量[tx, ty, theta]转换为对应2维平面中3*3齐次变换矩阵 113 | 114 | 115 | 116 | **重点实现函数**： 117 | 118 | ```C++ 119 | Eigen::Matrix3d icp::getTransform(const Eigen::MatrixXd &src, const Eigen::MatrixXd &tar) 120 | ``` 121 | 122 | 传入参数：getTransform函数读取两个点集矩阵src 和 tar。src为原始点集，tar为目标点集。src和tar结构均为：每行为点集中一个点在机器人局部坐标系下坐标（x, y），列数不限，代表传入需要匹配的点集中点数，src和tar列数应保持一致。 123 | 124 | 返回值：getTransform函数返回一个``Eigen::Matrix3d`` 的3x3 仿射变换矩阵，其结构为： 125 | 126 | ``` 127 | [Rstar2d tstar2d 128 | 0 0 1 ] 129 | ``` 130 | 131 | 其中Rstar2d为平面上2x2旋转矩阵，tstar2d为平面2x1位移向量 132 | 133 | 设src中点为p', tar中点为p，则应有``Rstar2d, tstar2d ∈ argmin ∑||p - (Rstar2d * p' + tstar2d)||²`` 134 | 135 | 算法实现： 136 | 137 | 根据线性代数知识，优化问题``Rstar2d ∈ argmin ∑||p - (Rstar2d * p' + tstar2d)||²``可用以下算法解决： 138 | 139 | 1. 求出两组点集质心位置：p_src和p_tar 140 | 2. 求出两组点集的去心坐标矩阵：demean_src和demean_tar 141 | 3. 令 W = demean_src^T * demean_tar 142 | 4. 对W做奇异值分解 W = USV^T 143 | 5. 则上述优化问题中Rstar2d = V * U^T 144 | 6. tstar2d = p_tar - Rstar2d * p_src 145 | 146 | 147 | 148 | ## 三、程序设计优化改进 149 | 150 | 对于助教所给的Bonus部分，我们进行了以下尝试： 151 | 152 | ### 1. 滤波算法 153 | 154 | pcl库当中提供了许多自带的滤波函数 155 | 156 | ```C++ 157 | #include //直通滤波器头文件 158 | #include //体素滤波器头文件 159 | #include //统计滤波器头文件 160 | #include //条件滤波器头文件 161 | #include //半径滤波器头文件 162 | ``` 163 | 164 | 考虑到本次实验中点云分布的性质，这里尝试采用统计滤波器对激光感应器采集到的点进行过滤，考虑到离群点的特征，则可以定义某处点云小于某个密度，既点云无效。计算每个点到其最近的k个点平均距离。则点云中所有点的距离应构成高斯分布。给定均值与方差，可剔除3∑之外的点。 165 | 166 | ```C++ 167 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 168 | for (int i = 0; i < src.size(); i++) 169 | { 170 | cloud->points[i].x = src[0,i]; 171 | cloud->points[i].y = src[1.i]; 172 | cloud->points[i].z = 0; 173 | } 174 | pcl::PointCloud::Ptr cloud_after_StatisticalRemoval(new pcl::PointCloud);// 175 | pcl::StatisticalOutlierRemoval Statistical; 176 | Statistical.setInputCloud(cloud); 177 | Statistical.setMeanK(20);//取平均值的临近点数 178 | Statistical.setStddevMulThresh(5);//临近点数数目少于多少时会被舍弃 179 | Statistical.filter(*cloud_after_StatisticalRemoval); 180 | ``` 181 | 182 | 这里将前一帧的点云在寻找最近邻之前进行过滤，得到的结果与后一帧进行匹配，实现剔除离群点的功能。 183 | 184 | 185 | 186 | ### 2. 点云地图构建 187 | 188 | **2.1 点云库的使用：** 189 | 190 | 1. pcl库安装 191 | 192 | 2. icp.cpp中包含头文件： 193 | 194 | ```C++ 195 | #include 196 | #include 197 | #include 198 | #include 199 | #include 200 | #include 201 | ``` 202 | 203 | 3. CMakeLists的添加： 204 | 205 | ``` 206 | find_package( PCL REQUIRED COMPONENT common io filters) 207 | FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization)include_directories(${PCL_INCLUDE_DIRS}) 208 | ``` 209 | 210 | 4. CMakeLists的修改： 211 | 212 | ``` 213 | target_link_libraries(tf 214 | ${catkin_LIBRARIES} 215 | ${PCL_LIBRARIES} 216 | ) 217 | ``` 218 | 219 | 220 | 221 | **2.2 点云图的输出**： 222 | 223 | 思路：在icp类中新增cloud成员用以储存点云地图，每次做icp迭代后记录其次变换矩阵，并与历史记录的变换矩阵累乘，得到从机器人初始位姿到当前位姿的齐次变换矩阵，将当前激光点云数据逆变换至初始位姿处，每个icp迭代循环中进行上述过程即可。 224 | 225 | 代码实现： 226 | 227 | 在icp::process()末尾添加如下代码： 228 | 229 | ```C++ 230 | //Point Cloud map building 231 | Eigen::MatrixXd moved_pc = PtTransform * src_pc; 232 | addVecToCloud(moved_pc); 233 | PtTransform = PtTransform * Transform_acc; 234 | ``` 235 | 236 | 其中addVecToCloud函数： 237 | 238 | ```C++ 239 | void icp::addVecToCloud(Eigen::MatrixXd moved_pc){ 240 | pcl::PointXYZ point; 241 | for(int i = 0; i < moved_pc.row(0).size(); i++){ 242 | point.x = moved_pc(0, i); 243 | point.y = moved_pc(1, i); 244 | point.z = 0;git@github.com:LeoDuhz/Advanced_Robotics.git 245 | cloud.push_back(point); 246 | } 247 | } 248 | ``` 249 | 250 | 251 | 252 | ## 四、实验结果讨论 253 | 254 | **利用rosbag测试icp算法** 255 | 256 | ```C++ 257 | $ roslaunch course_agv_icp icp.launch 258 | //另开终端 259 | $ rosbag play ****.bag 260 | ``` 261 | 262 | rviz中可见地图上显示出红色箭头表示icp的里程估计结果，与机器人局部坐标原点对比得到误差 263 | 264 | 265 | 266 | 运行course_agv.rviz，开始播放录制的rosbag 267 | 268 | 269 | 270 | 271 | 272 | 在rviz中，Global Options -> Fixed Frame改为world_base，程序将icp算法所计算的当前速度和方向显示在地图上 273 | 274 | 275 | 276 | 277 | 278 | 半途时，可见误差很小，icp里程估计测算路径与机器人实际经过路线基本吻合 279 | 280 | 281 | 282 | 283 | 284 | 在经过该处时里程估计误差稍有增大，主要原因是该点附近能见的障碍物和边界点较少，因此激光数据点较少，造成icp算法误差增大 285 | 286 | 287 | 288 | 289 | 290 | 对比滤波前程序在此处的误差，可以观察到由于滤波将两帧之间无法对应的点去掉，因此提高了icp算法的精准度，误差降低 291 | 292 | 293 | 294 | 295 | 296 | 路程结束回到起点，此时由于激光可见的障碍物点增多，icp精准度增高 297 | 298 | 299 | 300 | 301 | 302 | 最终里程估计误差放大图，可以看到误差已接近0 303 | 304 | 305 | 306 | -------------------------------------------------------------------------------- /icp/course_agv_icp/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/course_agv_icp/.DS_Store -------------------------------------------------------------------------------- /icp/course_agv_icp/.catkin_tools/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/course_agv_icp/.catkin_tools/CATKIN_IGNORE -------------------------------------------------------------------------------- /icp/course_agv_icp/.catkin_tools/README: -------------------------------------------------------------------------------- 1 | # Catkin Tools Metadata 2 | 3 | This directory was generated by catkin_tools and it contains persistent 4 | configuration information used by the `catkin` command and its sub-commands. 5 | 6 | Each subdirectory of the `profiles` directory contains a set of persistent 7 | configuration options for separate profiles. The default profile is called 8 | `default`. If another profile is desired, it can be described in the 9 | `profiles.yaml` file in this directory. 10 | 11 | Please see the catkin_tools documentation before editing any files in this 12 | directory. Most actions can be performed with the `catkin` command-line 13 | program. 14 | -------------------------------------------------------------------------------- /icp/course_agv_icp/.catkin_tools/VERSION: -------------------------------------------------------------------------------- 1 | 0.6.1 -------------------------------------------------------------------------------- /icp/course_agv_icp/.catkin_tools/profiles/default/build.yaml: -------------------------------------------------------------------------------- 1 | blacklist: [] 2 | build_space: build 3 | catkin_make_args: [] 4 | cmake_args: [] 5 | devel_layout: linked 6 | devel_space: devel 7 | extend_path: null 8 | install: false 9 | install_space: install 10 | isolate_install: false 11 | jobs_args: [] 12 | log_space: logs 13 | make_args: [] 14 | source_space: src 15 | use_env_cache: false 16 | use_internal_make_jobserver: true 17 | whitelist: [] 18 | -------------------------------------------------------------------------------- /icp/course_agv_icp/.catkin_tools/profiles/default/devel_collisions.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/course_agv_icp/.catkin_tools/profiles/default/devel_collisions.txt -------------------------------------------------------------------------------- /icp/course_agv_icp/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/devel_manifest.txt: -------------------------------------------------------------------------------- 1 | /home/leodu/course_agv_icp/build/catkin_tools_prebuild 2 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/local_setup.zsh /home/leodu/course_agv_icp/devel/./local_setup.zsh 3 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/setup.bash /home/leodu/course_agv_icp/devel/./setup.bash 4 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/setup.zsh /home/leodu/course_agv_icp/devel/./setup.zsh 5 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/cmake.lock /home/leodu/course_agv_icp/devel/./cmake.lock 6 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/local_setup.bash /home/leodu/course_agv_icp/devel/./local_setup.bash 7 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/local_setup.sh /home/leodu/course_agv_icp/devel/./local_setup.sh 8 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/_setup_util.py /home/leodu/course_agv_icp/devel/./_setup_util.py 9 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/setup.sh /home/leodu/course_agv_icp/devel/./setup.sh 10 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/env.sh /home/leodu/course_agv_icp/devel/./env.sh 11 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake /home/leodu/course_agv_icp/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig-version.cmake 12 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake /home/leodu/course_agv_icp/devel/share/catkin_tools_prebuild/cmake/catkin_tools_prebuildConfig.cmake 13 | /home/leodu/course_agv_icp/devel/.private/catkin_tools_prebuild/lib/pkgconfig/catkin_tools_prebuild.pc /home/leodu/course_agv_icp/devel/lib/pkgconfig/catkin_tools_prebuild.pc 14 | -------------------------------------------------------------------------------- /icp/course_agv_icp/.catkin_tools/profiles/default/packages/catkin_tools_prebuild/package.xml: -------------------------------------------------------------------------------- 1 | 2 | catkin_tools_prebuild 3 | 4 | This package is used to generate catkin setup files. 5 | 6 | 0.0.0 7 | BSD 8 | jbohren 9 | catkin 10 | 11 | -------------------------------------------------------------------------------- /icp/course_agv_icp/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build/ -------------------------------------------------------------------------------- /icp/course_agv_icp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(course_agv_icp) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | cmake_modules 7 | tf 8 | tf_conversions 9 | ) 10 | find_package(Eigen3 REQUIRED) 11 | #find_package(PCL 1.8 REQUIRED) 12 | find_package( PCL REQUIRED COMPONENT common io filters) 13 | FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization) 14 | 15 | add_definitions(-std=c++11 -O2 -g -DNDEBUG) 16 | 17 | catkin_package ( 18 | # CATKIN_DEPENDS roscpp 19 | ) 20 | 21 | 22 | include_directories(${PCL_INCLUDE_DIRS}) 23 | include_directories(include) 24 | include_directories( 25 | ${catkin_INCLUDE_DIRS} 26 | ${EIGEN_INCLUDE_DIRS} 27 | ) 28 | 29 | 30 | link_directories(${PCL_LIBRARY_DIRS}) 31 | add_definitions(${PCL_DEFINITIONS}) 32 | 33 | 34 | add_executable(icp src/icp.cpp) 35 | target_link_libraries(icp 36 | ${catkin_LIBRARIES} 37 | ${PCL_LIBRARIES} 38 | ) 39 | 40 | add_executable(tf src/tf.cpp) 41 | target_link_libraries(tf 42 | ${catkin_LIBRARIES} 43 | ${PCL_LIBRARIES} 44 | ) 45 | -------------------------------------------------------------------------------- /icp/course_agv_icp/launch/course_agv.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /RobotModel1 11 | - /TF1 12 | - /Map1 13 | - /LaserScan1 14 | - /Imu1 15 | - /Odometry1 16 | - /MarkerArray1 17 | - /Map2 18 | - /Map2/Position1 19 | - /MarkerArray2 20 | Splitter Ratio: 0.5 21 | Tree Height: 711 22 | - Class: rviz/Selection 23 | Name: Selection 24 | - Class: rviz/Tool Properties 25 | Expanded: 26 | - /2D Pose Estimate1 27 | - /2D Nav Goal1 28 | - /Publish Point1 29 | Name: Tool Properties 30 | Splitter Ratio: 0.588679 31 | - Class: rviz/Views 32 | Expanded: 33 | - /Current View1 34 | Name: Views 35 | Splitter Ratio: 0.5 36 | - Class: rviz/Time 37 | Experimental: false 38 | Name: Time 39 | SyncMode: 0 40 | SyncSource: "" 41 | Visualization Manager: 42 | Class: "" 43 | Displays: 44 | - Alpha: 0.5 45 | Cell Size: 1 46 | Class: rviz/Grid 47 | Color: 255; 255; 255 48 | Enabled: true 49 | Line Style: 50 | Line Width: 0.03 51 | Value: Lines 52 | Name: Grid 53 | Normal Cell Count: 0 54 | Offset: 55 | X: 0 56 | Y: 0 57 | Z: 0 58 | Plane: XY 59 | Plane Cell Count: 20 60 | Reference Frame: 61 | Value: true 62 | - Alpha: 1 63 | Class: rviz/RobotModel 64 | Collision Enabled: false 65 | Enabled: true 66 | Links: 67 | All Links Enabled: true 68 | Expand Joint Details: false 69 | Expand Link Details: false 70 | Expand Tree: false 71 | Link Tree Style: Links in Alphabetic Order 72 | Name: RobotModel 73 | Robot Description: robot_description 74 | TF Prefix: "" 75 | Update Interval: 0 76 | Value: true 77 | Visual Enabled: true 78 | - Class: rviz/TF 79 | Enabled: true 80 | Frame Timeout: 15 81 | Frames: 82 | All Enabled: true 83 | course_agv__hokuyo__link: 84 | Value: true 85 | course_agv__imu: 86 | Value: true 87 | course_agv__left_wheel: 88 | Value: true 89 | course_agv__right_wheel: 90 | Value: true 91 | icp_odom: 92 | Value: true 93 | map: 94 | Value: true 95 | pf_loc: 96 | Value: true 97 | robot_base: 98 | Value: true 99 | robot_chassis: 100 | Value: true 101 | world_base: 102 | Value: true 103 | Marker Scale: 0.3 104 | Name: TF 105 | Show Arrows: true 106 | Show Axes: true 107 | Show Names: true 108 | Tree: 109 | world_base: 110 | icp_odom: 111 | {} 112 | map: 113 | pf_loc: 114 | {} 115 | robot_base: 116 | course_agv__hokuyo__link: 117 | {} 118 | course_agv__imu: 119 | {} 120 | course_agv__left_wheel: 121 | {} 122 | course_agv__right_wheel: 123 | {} 124 | robot_chassis: 125 | {} 126 | Update Interval: 0 127 | Value: true 128 | - Alpha: 0.5 129 | Class: rviz/Map 130 | Color Scheme: map 131 | Draw Behind: false 132 | Enabled: true 133 | Name: Map 134 | Topic: /map 135 | Unreliable: false 136 | Value: true 137 | - Alpha: 0.6 138 | Autocompute Intensity Bounds: true 139 | Autocompute Value Bounds: 140 | Max Value: 10 141 | Min Value: -10 142 | Value: true 143 | Axis: Z 144 | Channel Name: intensity 145 | Class: rviz/LaserScan 146 | Color: 255; 255; 255 147 | Color Transformer: Intensity 148 | Decay Time: 0 149 | Enabled: false 150 | Invert Rainbow: false 151 | Max Color: 255; 255; 255 152 | Max Intensity: 0 153 | Min Color: 0; 0; 0 154 | Min Intensity: 0 155 | Name: LaserScan 156 | Position Transformer: XYZ 157 | Queue Size: 10 158 | Selectable: true 159 | Size (Pixels): 3 160 | Size (m): 0.05 161 | Style: Flat Squares 162 | Topic: /course_agv/laser/scan 163 | Unreliable: false 164 | Use Fixed Frame: true 165 | Use rainbow: true 166 | Value: false 167 | - Alpha: 0.2 168 | Class: rviz_plugin_tutorials/Imu 169 | Color: 32; 74; 135 170 | Enabled: false 171 | History Length: 1 172 | Name: Imu 173 | Topic: /course_agv/imu 174 | Unreliable: false 175 | Value: false 176 | - Angle Tolerance: 0.1 177 | Class: rviz/Odometry 178 | Color: 255; 255; 0 179 | Enabled: true 180 | Keep: 1000 181 | Length: 0.1 182 | Name: Odometry 183 | Position Tolerance: 0.1 184 | Topic: /icp_odom 185 | Value: true 186 | - Class: rviz/MarkerArray 187 | Enabled: true 188 | Marker Topic: /landMarks 189 | Name: MarkerArray 190 | Namespaces: 191 | lm: true 192 | Queue Size: 100 193 | Value: true 194 | - Alpha: 0.7 195 | Class: rviz/Map 196 | Color Scheme: map 197 | Draw Behind: false 198 | Enabled: false 199 | Name: Map 200 | Topic: /grid_map 201 | Unreliable: false 202 | Value: false 203 | - Class: rviz/MarkerArray 204 | Enabled: true 205 | Marker Topic: /particles 206 | Name: MarkerArray 207 | Namespaces: 208 | particle: true 209 | Queue Size: 100 210 | Value: true 211 | Enabled: true 212 | Global Options: 213 | Background Color: 85; 87; 83 214 | Fixed Frame: map 215 | Frame Rate: 30 216 | Name: root 217 | Tools: 218 | - Class: rviz/Interact 219 | Hide Inactive Objects: true 220 | - Class: rviz/MoveCamera 221 | - Class: rviz/Select 222 | - Class: rviz/FocusCamera 223 | - Class: rviz/Measure 224 | - Class: rviz/SetInitialPose 225 | Topic: /initialpose 226 | - Class: rviz/SetGoal 227 | Topic: /move_base_simple/goal 228 | - Class: rviz/PublishPoint 229 | Single click: true 230 | Topic: /clicked_point 231 | Value: true 232 | Views: 233 | Current: 234 | Angle: 4.40501 235 | Class: rviz/TopDownOrtho 236 | Enable Stereo Rendering: 237 | Stereo Eye Separation: 0.06 238 | Stereo Focal Distance: 1 239 | Swap Stereo Eyes: false 240 | Value: false 241 | Name: Current View 242 | Near Clip Distance: 0.01 243 | Scale: -166.013 244 | Target Frame: 245 | Value: TopDownOrtho (rviz) 246 | X: 0.11201 247 | Y: -0.168376 248 | Saved: ~ 249 | Window Geometry: 250 | Displays: 251 | collapsed: false 252 | Height: 992 253 | Hide Left Dock: false 254 | Hide Right Dock: false 255 | QMainWindow State: 000000ff00000000fd00000004000000000000018c00000356fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000356000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000356fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000356000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007570000003efc0100000002fb0000000800540069006d0065010000000000000757000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004b00000035600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 256 | Selection: 257 | collapsed: false 258 | Time: 259 | collapsed: false 260 | Tool Properties: 261 | collapsed: false 262 | Views: 263 | collapsed: false 264 | Width: 1879 265 | X: 185 266 | Y: 130 267 | -------------------------------------------------------------------------------- /icp/course_agv_icp/launch/icp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /icp/course_agv_icp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | course_agv_icp 4 | 0.0.0 5 | The course_agv_icp package 6 | 7 | 8 | 9 | 10 | zjuyiunhuan 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /icp/course_agv_icp/src/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/course_agv_icp/src/.DS_Store -------------------------------------------------------------------------------- /icp/course_agv_icp/src/tf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int main(int argc, char** argv) { 7 | ros::init(argc, argv, "my_tf"); 8 | ros::NodeHandle nh; 9 | tf::StampedTransform tf_stamped; 10 | tf::TransformBroadcaster tf_broadcaster; 11 | try{ 12 | tf::TransformListener tf_listener; 13 | tf_listener.waitForTransform("/map", "/base_footprint", ros::Time(), ros::Duration(10.0)); 14 | tf_listener.lookupTransform("/map", "/base_footprint", ros::Time(0), tf_stamped); 15 | tf_stamped.frame_id_ = "map"; 16 | tf_stamped.child_frame_id_ = "world_base"; 17 | ros::Rate rate(20); 18 | while(ros::ok()) { 19 | tf_stamped.stamp_ = ros::Time::now(); 20 | tf_broadcaster.sendTransform(tf_stamped); 21 | rate.sleep(); 22 | } 23 | } 24 | catch(tf::TransformException& ex) { 25 | ROS_ERROR("ERROR"); 26 | } 27 | } -------------------------------------------------------------------------------- /icp/images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/1.png -------------------------------------------------------------------------------- /icp/images/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2.png -------------------------------------------------------------------------------- /icp/images/2020-08-11 15-01-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-11 15-01-05屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-12 23-21-35屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-12 23-21-35屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-15 11-41-24屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-15 11-41-24屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-17 11-13-15屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-17 11-13-15屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-18 22-34-54屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-18 22-34-54屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-19 10-57-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-19 10-57-05屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-19 14-58-22屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-19 14-58-22屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-19 14-58-48屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-19 14-58-48屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-19 14-59-16屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-19 14-59-16屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-19 15-04-03屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-19 15-04-03屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-19 15-09-58屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-19 15-09-58屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-19 15-12-31屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-19 15-12-31屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-19 15-12-47屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-19 15-12-47屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-19 15-15-48屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-19 15-15-48屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-23-13屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-23-13屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-25-01屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-25-01屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-30-08屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-30-08屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-30-19屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-30-19屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-32-22屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-32-22屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-32-37屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-32-37屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-35-12屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-35-12屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-36-23屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-36-23屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-37-23屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-37-23屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 10-40-06屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 10-40-06屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 13-57-03屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 13-57-03屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 13-57-22屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 13-57-22屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 14-01-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 14-01-05屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-21 14-04-46屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-21 14-04-46屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-23 21-23-14屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-23 21-23-14屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-23 22-05-52屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-23 22-05-52屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-24 20-45-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-24 20-45-05屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-24 21-05-44屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-24 21-05-44屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-24 21-06-18屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-24 21-06-18屏幕截图.png -------------------------------------------------------------------------------- /icp/images/2020-08-24 21-17-47屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/2020-08-24 21-17-47屏幕截图.png -------------------------------------------------------------------------------- /icp/images/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/3.png -------------------------------------------------------------------------------- /icp/images/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/4.png -------------------------------------------------------------------------------- /icp/images/after shrink.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/after shrink.png -------------------------------------------------------------------------------- /icp/images/before shrink.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/before shrink.png -------------------------------------------------------------------------------- /icp/images/icp0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/icp0.png -------------------------------------------------------------------------------- /icp/images/icp00.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/icp00.png -------------------------------------------------------------------------------- /icp/images/icp1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/icp1.png -------------------------------------------------------------------------------- /icp/images/icp2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/icp2.png -------------------------------------------------------------------------------- /icp/images/icp3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/icp3.png -------------------------------------------------------------------------------- /icp/images/icp4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/icp4.png -------------------------------------------------------------------------------- /icp/images/icp5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/icp/images/icp5.png -------------------------------------------------------------------------------- /robot_arm/Document.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/Document.pdf -------------------------------------------------------------------------------- /robot_arm/images/2020-08-11 15-01-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-11 15-01-05屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-12 23-21-35屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-12 23-21-35屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-15 11-41-24屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-15 11-41-24屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-17 11-13-15屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-17 11-13-15屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-18 22-34-54屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-18 22-34-54屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-19 10-57-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-19 10-57-05屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-19 14-58-22屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-19 14-58-22屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-19 14-58-48屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-19 14-58-48屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-19 14-59-16屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-19 14-59-16屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-19 15-04-03屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-19 15-04-03屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-19 15-09-58屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-19 15-09-58屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-19 15-12-31屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-19 15-12-31屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-19 15-12-47屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-19 15-12-47屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-19 15-15-48屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-19 15-15-48屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-22-28屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-22-28屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-23-13屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-23-13屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-25-01屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-25-01屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-30-08屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-30-08屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-30-19屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-30-19屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-32-22屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-32-22屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-32-37屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-32-37屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-35-12屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-35-12屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-36-23屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-36-23屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-37-23屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-37-23屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 10-40-06屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 10-40-06屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 13-57-03屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 13-57-03屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 13-57-22屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 13-57-22屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 14-01-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 14-01-05屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/2020-08-21 14-04-46屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/2020-08-21 14-04-46屏幕截图.png -------------------------------------------------------------------------------- /robot_arm/images/README.md: -------------------------------------------------------------------------------- 1 | **markdown格式报告涉及到的图片** 2 | -------------------------------------------------------------------------------- /robot_arm/images/after shrink.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/after shrink.png -------------------------------------------------------------------------------- /robot_arm/images/before shrink.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/before shrink.png -------------------------------------------------------------------------------- /robot_arm/images/点到点轨迹规划.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/robot_arm/images/点到点轨迹规划.png -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/.idea/probot_gazebo.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 1582077880708 49 | 62 | 63 | 64 | 65 | 67 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(probot_gazebo) 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 | moveit_msgs 14 | moveit_ros_perception 15 | moveit_ros_planning_interface 16 | trajectory_msgs 17 | probot_msgs 18 | probot_anno_ikfast_manipulator_plugin 19 | message_generation 20 | std_msgs 21 | ) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | # find_package(Boost REQUIRED COMPONENTS system) 25 | 26 | 27 | ## Uncomment this if the package has a setup.py. This macro ensures 28 | ## modules and global scripts declared therein get installed 29 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 30 | # catkin_python_setup() 31 | 32 | ################################################ 33 | ## Declare ROS messages, services and actions ## 34 | ################################################ 35 | 36 | ## To declare and build messages, services or actions from within this 37 | ## package, follow these steps: 38 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 39 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 40 | ## * In the file package.xml: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 43 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 44 | ## but can be declared for certainty nonetheless: 45 | ## * add a exec_depend tag for "message_runtime" 46 | ## * In this file (CMakeLists.txt): 47 | ## * add "message_generation" and every package in MSG_DEP_SET to 48 | ## find_package(catkin REQUIRED COMPONENTS ...) 49 | ## * add "message_runtime" and every package in MSG_DEP_SET to 50 | ## catkin_package(CATKIN_DEPENDS ...) 51 | ## * uncomment the add_*_files sections below as needed 52 | ## and list every .msg/.srv/.action file to be processed 53 | ## * uncomment the generate_messages entry below 54 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 55 | 56 | ## Generate messages in the 'msg' folder 57 | # add_message_files( 58 | # FILES 59 | # Message1.msg 60 | # Message2.msg 61 | # ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.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 # Or other packages containing msgs 81 | # ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if your package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | # INCLUDE_DIRS include 114 | # LIBRARIES probot_gazebo 115 | # CATKIN_DEPENDS other_catkin_pkg 116 | CATKIN_DEPENDS roscpp rospy moveit_core moveit_ros_planning_interface probot_msgs probot_anno_ikfast_manipulator_plugin std_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 | /home/leodu/catkin_ws1/src/probot_anno/probot_gazebo/include 129 | ${catkin_INCLUDE_DIRS} 130 | ) 131 | 132 | ## Declare a C++ library 133 | # add_library(${PROJECT_NAME} 134 | # src/${PROJECT_NAME}/probot_gazebo.cpp 135 | # ) 136 | 137 | ## Add cmake target dependencies of the library 138 | ## as an example, code may need to be generated before libraries 139 | ## either from message generation or dynamic reconfigure 140 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 141 | 142 | ## Declare a C++ executable 143 | ## With catkin_make all packages are built within a single CMake context 144 | ## The recommended prefix ensures that target names across packages don't collide 145 | # add_executable(${PROJECT_NAME}_node src/probot_gazebo_node.cpp) 146 | 147 | ## Rename C++ executable without prefix 148 | ## The above recommended prefix causes long target names, the following renames the 149 | ## target back to the shorter version for ease of user use 150 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 151 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 152 | 153 | ## Add cmake target dependencies of the executable 154 | ## same as for the library above 155 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 156 | 157 | ## Specify libraries to link a library or executable target against 158 | # target_link_libraries(${PROJECT_NAME}_node 159 | # ${catkin_LIBRARIES} 160 | # ) 161 | 162 | add_executable(control_example src/control_example.cpp) 163 | target_link_libraries(control_example ${catkin_LIBRARIES}) 164 | 165 | add_executable(control_robot_vel src/control_robot_vel.cpp) 166 | target_link_libraries(control_robot_vel ${catkin_LIBRARIES}) 167 | 168 | add_executable(trajectory_planning src/trajectory_planning.cpp) 169 | target_link_libraries(trajectory_planning ${catkin_LIBRARIES}) 170 | 171 | add_executable(trajwithobstacle src/trajwithobstacle.cpp) 172 | target_link_libraries(trajwithobstacle ${catkin_LIBRARIES}) 173 | 174 | add_executable(trajwithobstacle_real src/trajwithobstacle_real.cpp) 175 | target_link_libraries(trajwithobstacle_real ${catkin_LIBRARIES}) 176 | 177 | ############# 178 | ## Install ## 179 | ############# 180 | 181 | # all install targets should use catkin DESTINATION variables 182 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 183 | 184 | ## Mark executable scripts (Python etc.) for installation 185 | ## in contrast to setup.py, you can choose the destination 186 | # install(PROGRAMS 187 | # scripts/my_python_script 188 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 189 | # ) 190 | 191 | ## Mark executables and/or libraries for installation 192 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 193 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 194 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 195 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 196 | # ) 197 | 198 | ## Mark cpp header files for installation 199 | # install(DIRECTORY include/${PROJECT_NAME}/ 200 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 201 | # FILES_MATCHING PATTERN "*.h" 202 | # PATTERN ".svn" EXCLUDE 203 | # ) 204 | 205 | ## Mark other files for installation (e.g. launch and bag files, etc.) 206 | # install(FILES 207 | # # myfile1 208 | # # myfile2 209 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 210 | # ) 211 | 212 | install(DIRECTORY 213 | config 214 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 215 | ) 216 | 217 | install(DIRECTORY 218 | launch 219 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 220 | ) 221 | 222 | ############# 223 | ## Testing ## 224 | ############# 225 | 226 | ## Add gtest based cpp test target and link libraries 227 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_probot_gazebo.cpp) 228 | # if(TARGET ${PROJECT_NAME}-test) 229 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 230 | # endif() 231 | 232 | ## Add folders to be run by python nosetests 233 | # catkin_add_nosetests(test) 234 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/config/probot_anno_control.yaml: -------------------------------------------------------------------------------- 1 | probot_anno: 2 | arm_pos_controller: 3 | type: "position_controllers/JointGroupPositionController" 4 | joints: 5 | - joint_1 6 | - joint_2 7 | - joint_3 8 | - joint_4 9 | - joint_5 10 | - joint_6 11 | 12 | gains: 13 | joint_1: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 14 | joint_2: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 15 | joint_3: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 16 | joint_4: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 17 | joint_5: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 18 | joint_6: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 19 | 20 | arm_vel_controller: 21 | type: "velocity_controllers/JointGroupVelocityController" 22 | joints: 23 | - joint_1 24 | - joint_2 25 | - joint_3 26 | - joint_4 27 | - joint_5 28 | - joint_6 29 | 30 | gains: 31 | joint_1: {p: 1000.0, i: 0.0, d: 0.1} 32 | joint_2: {p: 1000.0, i: 0.0, d: 0.1} 33 | joint_3: {p: 1000.0, i: 0.0, d: 0.1} 34 | joint_4: {p: 1000.0, i: 0.0, d: 0.1} 35 | joint_5: {p: 1000.0, i: 0.0, d: 0.1} 36 | joint_6: {p: 1000.0, i: 0.0, d: 0.1} 37 | 38 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/config/probot_anno_gazebo_joint_states.yaml: -------------------------------------------------------------------------------- 1 | probot_anno: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/config/probot_anno_position_control.yaml: -------------------------------------------------------------------------------- 1 | probot_anno: 2 | 3 | arm_pos_controller: 4 | type: "position_controllers/JointGroupPositionController" 5 | joints: 6 | - joint_1 7 | - joint_2 8 | - joint_3 9 | - joint_4 10 | - joint_5 11 | - joint_6 12 | 13 | gains: 14 | joint_1: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 15 | joint_2: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 16 | joint_3: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 17 | joint_4: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 18 | joint_5: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 19 | joint_6: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 20 | 21 | 22 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/config/probot_anno_velocity_control.yaml: -------------------------------------------------------------------------------- 1 | probot_anno: 2 | arm_vel_controller: 3 | type: "velocity_controllers/JointGroupVelocityController" 4 | joints: 5 | - joint_1 6 | - joint_2 7 | - joint_3 8 | - joint_4 9 | - joint_5 10 | - joint_6 11 | 12 | gains: 13 | joint_1: {p: 1000.0, i: 0.0, d: 0.1} 14 | joint_2: {p: 1000.0, i: 0.0, d: 0.1} 15 | joint_3: {p: 1000.0, i: 0.0, d: 0.1} 16 | joint_4: {p: 1000.0, i: 0.0, d: 0.1} 17 | joint_5: {p: 1000.0, i: 0.0, d: 0.1} 18 | joint_6: {p: 1000.0, i: 0.0, d: 0.1} 19 | 20 | 21 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_position_control/planning_context_pos.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_position_control/probot_anno_gazebo_states.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_position_control/probot_anno_gazebo_world_pos.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_position_control/probot_anno_position_control_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | [move_group/fake_controller_joint_states] 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_position_control/probot_anno_position_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control/planning_context_vel.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control/probot_anno_gazebo_states.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control/probot_anno_gazebo_world_vel.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control/probot_anno_velocity_control_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | [move_group/fake_controller_joint_states] 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 35 | 36 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control/probot_anno_velocity_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control_ring/planning_context_vel.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control_ring/probot_anno_gazebo_states.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control_ring/probot_anno_gazebo_world_vel.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control_ring/probot_anno_velocity_control_ring_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | [move_group/fake_controller_joint_states] 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 35 | 36 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/launch/probot_anno_velocity_control_ring/probot_anno_velocity_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | probot_gazebo 4 | 0.0.0 5 | The probot_gazebo package 6 | 7 | 8 | 9 | 10 | Chunxu Hu 11 | 12 | 13 | 14 | 15 | 16 | Apache V2.0 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 | moveit_core 55 | moveit_ros_planning_interface 56 | probot_msgs 57 | probot_anno_ikfast_manipulator_plugin 58 | std_msgs 59 | message_generation 60 | 61 | roscpp 62 | rospy 63 | roscpp 64 | rospy 65 | moveit_core 66 | moveit_ros_planning_interface 67 | probot_msgs 68 | probot_anno_ikfast_manipulator_plugin 69 | std_msgs 70 | message_runtime 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/src/DK.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | using namespace std; 8 | using namespace Eigen; 9 | 10 | #define pi 3.1415926535897932384626 11 | 12 | class DK{ 13 | public: 14 | Eigen::MatrixXd calDK(); 15 | Eigen::MatrixXd calT(double alpha, double a, double d, double w); 16 | double w[6]{0}; 17 | 18 | private: 19 | 20 | }; 21 | 22 | Eigen::MatrixXd DK::calT(double alpha, double a, double d, double w){ 23 | Eigen::MatrixXd T = Eigen::MatrixXd::Ones(4,4); 24 | 25 | T(0,0) = std::cos(w); 26 | T(0,1) = -std::sin(w); 27 | T(0,2) = 0; 28 | T(0,3) = a; 29 | T(1,0) = std::sin(w)*std::cos(alpha); 30 | T(1,1) = std::cos(w)*std::cos(alpha); 31 | T(1,2) = -std::sin(alpha); 32 | T(1,3) = -std::sin(alpha)*d; 33 | T(2,0) = std::sin(w)*std::sin(alpha); 34 | T(2,1) = std::cos(w)*std::sin(alpha); 35 | T(2,2) = std::cos(alpha); 36 | T(2,3) = cos(alpha)*d; 37 | T(3,0) = 0; 38 | T(3,1) = 0; 39 | T(3,2) = 0; 40 | T(3,3) = 1; 41 | 42 | return T; 43 | 44 | } 45 | 46 | Eigen::MatrixXd DK::calDK() 47 | { 48 | Eigen::MatrixXd T = Eigen::MatrixXd::Identity(4,4); 49 | 50 | T = T * calT(0,0,0.284,this->w[0]); 51 | T = T * calT(pi/2,0,0,pi/2+this->w[1]); 52 | T = T * calT(0,0.225,0,this->w[2]); 53 | T = T * calT(pi/2,0,0.2289,this->w[3]); 54 | T = T * calT(-pi/2,0,0,-pi/2+this->w[4]); 55 | T = T * calT(pi/2,0,0.055,this->w[5]); 56 | T = T * calT(-pi/2,0,0,0); 57 | 58 | return T; 59 | } 60 | int main(int argc, char **argv) 61 | { 62 | DK dk; 63 | for (int i=0;i<6;i++) 64 | { 65 | std::cin>>dk.w[i]; 66 | } 67 | 68 | Eigen::MatrixXd T = dk.calDK(); 69 | Eigen::MatrixXd xyz = T.block<3,1>(0,3).transpose(); 70 | double pitch = asin(-T(2,0)); 71 | double yaw = asin(T(1,0)/std::cos(pitch)); 72 | double roll = acos(T(2,2)/std::cos(pitch)); 73 | 74 | std::cout<<"x,y,z: "< 10 | #include 11 | #include 12 | #include 13 | #include "vector" 14 | #include "std_msgs/Float32.h" 15 | #include "std_msgs/Float64MultiArray.h" 16 | #include "controller_manager_msgs/SwitchController.h" 17 | #include "controller_manager_msgs/ListControllers.h" 18 | #include "ikfast.h" 19 | #include "probot_anno_manipulator_ikfast_moveit_plugin.cpp" 20 | 21 | using namespace ikfast_kinematics_plugin; 22 | int main(int argc, char **argv) { 23 | 24 | bool ret; 25 | //节点初始化 26 | ros::init(argc, argv, "control_example"); 27 | //创建节点句柄对象 28 | ros::NodeHandle node_handle; 29 | ros::AsyncSpinner spinner(1); 30 | ROS_INFO_STREAM("start"); 31 | //创建发布者对象，用于发布位置信息， 32 | //位置控制的话题名为："/probot_anno/arm_pos_controller/command" 33 | //速度控制的话题名为："/probot_anno/arm_vel_controller/command" 34 | //发送的数据类型均为：std_msgs::Float64MultiArray， 35 | //它的创建与赋值方法在下面有，一组6个浮点数 36 | ros::Publisher pos_pub = node_handle.advertise("/probot_anno/arm_pos_controller/command", 100); 37 | 38 | //以下为ikfast运动学求解器的使用，要想仔细了解，自己看它的类的头文件，注释只解释必要的内容 39 | //创建运动学类实例ik 40 | ikfast_kinematics_plugin::IKFastKinematicsPlugin ik; 41 | //初始化对象，参数是机械臂模型相关的一些内容，link_6即末端连杆，它的坐标系所在位置就是末端中心点，与PPT图片里的一致 42 | ret = ik.IKFastKinematicsPlugin::initialize("robot_description","manipulator","base_link","link_6",0.001); 43 | // 设置机器人终端的目标位置 44 | //geometry_msgs::Pose 这个类就是用来装机械臂位置姿态的， 45 | // 它的position成员里装3个坐标，orientation成员里装x,y,z,w，这是四元数位姿表示法 46 | //当x=1时，末端处于初始位姿，对应的仿真机器人欧拉角姿态为（1.57, 0, 0） 47 | geometry_msgs::Pose target_pose; 48 | target_pose.position.x = 0.2; 49 | target_pose.position.y = 0; 50 | target_pose.position.z = 0.3; 51 | target_pose.orientation.x = 1.0; 52 | 53 | //pose1为vector类型变量，装入上述位姿信息 54 | std::vector pose1; 55 | pose1.push_back(target_pose); 56 | 57 | //设定关节角参考初值，这是求解函数要求的一个参数，默认为0即可，是vector类型 58 | std::vector seed1; 59 | seed1.push_back(0.0); 60 | seed1.push_back(0.0); 61 | seed1.push_back(0.0); 62 | seed1.push_back(0.0); 63 | seed1.push_back(0.0); 64 | seed1.push_back(0.0); 65 | 66 | //创建用于保存逆解的变量，它是vector矩阵，vector>类型，因为会解出不止一组解装在里面 67 | //内容六关节角度，每个角度是浮点数，为弧度制 68 | std::vector> sol_rad; 69 | //这是求解函数要求的一个参数，具体功能不明 70 | kinematics::KinematicsResult kinematic_result; 71 | 72 | //计算逆解 73 | //参数表：（末端位姿，参考关节角初值，关节角，......） 74 | ret = ik.getPositionIK(pose1, seed1, sol_rad, kinematic_result, kinematics::KinematicsQueryOptions()); 75 | 76 | if(ret)//若求解成功，输出结果 77 | { 78 | std::cout << sol_rad.size() << " IK solved successfully." << endl; 79 | //角度制计算，并输出每一组计算结果 80 | for (int q = 0; q < sol_rad.size(); q++) 81 | { 82 | for (int i = 0; i < 6; i++) 83 | { 84 | cout << sol_rad[q][i]<<" "; 85 | } 86 | cout << endl; 87 | } 88 | } 89 | 90 | //初始化publish的变量并初始化6个值 91 | std_msgs::Float64MultiArray init_pos; 92 | init_pos.data.push_back(0); 93 | init_pos.data.push_back(0); 94 | init_pos.data.push_back(0); 95 | init_pos.data.push_back(0); 96 | init_pos.data.push_back(0); 97 | init_pos.data.push_back(0); 98 | sleep(1); 99 | //double targetPose[6] = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1};//运动的起点位置 100 | //double targetPose[6] = {0, -1.5708, 1.5708, 0, 1.5708, 0};//运动的起点位置 101 | 102 | //为要发送的变量装入解出的六关节坐标 103 | //如果是要发送速度，那也是类似于这样，装入6个关节角速度值（弧度制）即可 104 | init_pos.data.at(0) = sol_rad[0][0]; 105 | init_pos.data.at(1) = sol_rad[0][1]; 106 | init_pos.data.at(2) = sol_rad[0][2]; 107 | init_pos.data.at(3) = sol_rad[0][3]; 108 | init_pos.data.at(4) = sol_rad[0][4]; 109 | init_pos.data.at(5) = sol_rad[0][5]; 110 | 111 | //发送出去，若成功，机械臂状态会改变 112 | pos_pub.publish(init_pos); 113 | ROS_INFO_STREAM("published"); 114 | } 115 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/src/control_robot_vel.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "vector" 6 | #include "std_msgs/Float32.h" 7 | #include "std_msgs/Float64MultiArray.h" 8 | #include "controller_manager_msgs/SwitchController.h" 9 | #include "controller_manager_msgs/ListControllers.h" 10 | #include "probot_anno_manipulator_ikfast_moveit_plugin.cpp" 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #define pi 3.1415926535897932384626 18 | using namespace std; 19 | class Anno_robot{ 20 | public: 21 | Anno_robot(Eigen::MatrixXd DHparam); 22 | ~Anno_robot() {}; 23 | Eigen::VectorXd getJointState(); 24 | void setEndVelocity(Eigen::VectorXd); 25 | Eigen::MatrixXd getJacobian(); 26 | Eigen::MatrixXd calcTransform(double a, double b, double d, double o); 27 | void printParam(); 28 | static double timestep; 29 | Eigen::VectorXd joint_velocity; 30 | 31 | private: 32 | Eigen::MatrixXd DHparam; 33 | Eigen::VectorXd end_velocity; 34 | 35 | }; 36 | 37 | double Anno_robot::timestep = 0.02; 38 | 39 | Anno_robot::Anno_robot(Eigen::MatrixXd DHparam) { 40 | this->DHparam = DHparam; 41 | end_velocity = Eigen::VectorXd::Zero(6,1); 42 | joint_velocity = Eigen::VectorXd::Zero(6,1); 43 | } 44 | 45 | Eigen::VectorXd Anno_robot::getJointState() { 46 | return DHparam.block<6, 1>(0, 3); 47 | } 48 | 49 | void Anno_robot::setEndVelocity(Eigen::VectorXd V) { 50 | this->end_velocity = V; 51 | Eigen::MatrixXd J = this->getJacobian(); 52 | 53 | if (J.determinant() != 0) { 54 | this->joint_velocity = J.inverse() * V; 55 | } 56 | else { 57 | std::cout << "�ſɱȾ������죡�޷������ٶ�" << std::endl; 58 | //try { 59 | //throw 0; 60 | //} 61 | Eigen::VectorXd delta; 62 | } 63 | 64 | Eigen::VectorXd delta_joint_state = Anno_robot::timestep * this->joint_velocity; 65 | DHparam.block<6, 1>(0, 3) += delta_joint_state; 66 | 67 | } 68 | 69 | Eigen::MatrixXd Anno_robot::getJacobian(){ 70 | Eigen::MatrixXd P(3, 6); 71 | Eigen::MatrixXd Z(3, 6); 72 | Eigen::MatrixXd T = Eigen::MatrixXd::Identity(4, 4); 73 | 74 | for (int i = 0; i < 6; i++) { 75 | T = T * calcTransform(DHparam(i,0), DHparam(i,1), DHparam(i,2), DHparam(i,3)); 76 | P.block<3, 1>(0, i) = T.block<3, 1>(0, 3); 77 | Z.block<3, 1>(0, i) = T.block<3, 1>(0, 2); 78 | 79 | } 80 | 81 | Eigen::MatrixXd J(6, 6); 82 | Eigen::Vector3d z(3); 83 | Eigen::Vector3d p(3); 84 | Eigen::Vector3d pN = P.block<3, 1>(0, 5); 85 | 86 | for (int i = 0; i < 5; i++){ 87 | z = Z.block<3, 1>(0, i); 88 | p = P.block<3, 1>(0, i); 89 | J.block<3, 1>(0, i) = z.cross(pN - p); 90 | J.block<3, 1>(3, i) = z; 91 | } 92 | J.block<3, 1>(0, 5) = Eigen::VectorXd::Zero(3, 1); 93 | J.block<3, 1>(3, 5) = Z.block<3, 1>(0, 5); 94 | 95 | return J; 96 | } 97 | 98 | Eigen::MatrixXd Anno_robot::calcTransform(double b, double a, double d, double o) 99 | { 100 | Eigen::MatrixXd T(4, 4); // T[i-1 -> i] 101 | T << cos(o), -sin(o), 0, a, 102 | sin(o)* cos(b), cos(o)* cos(b), -sin(b), -sin(b) * d, 103 | sin(o)* sin(b), cos(o)* sin(b), cos(b), cos(b)* d, 104 | 0, 0, 0, 1; 105 | return T; 106 | } 107 | 108 | void Anno_robot::printParam() 109 | { 110 | 111 | Eigen::MatrixXd T = Eigen::MatrixXd::Identity(4,4); 112 | 113 | for (int i = 0; i < 6; i++) { 114 | T = T * calcTransform(DHparam(i, 0), DHparam(i, 1), DHparam(i, 2), DHparam(i, 3)); 115 | } 116 | 117 | Eigen::MatrixXd Final(4, 4); 118 | Final << 1, 0, 0, 0, 119 | 0, 0, 1, 0, 120 | 0, -1, 0, 0, 121 | 0, 0, 0, 1; 122 | T = T * Final; 123 | 124 | std::cout << "transformation matrix T:" << std::endl; 125 | std::cout << T << std::endl; 126 | 127 | double yaw, pitch, roll; 128 | pitch = -asin(T(2, 0)); 129 | yaw = asin(T(1, 0) / cos(pitch)); 130 | roll = asin(T(2, 1) / cos(pitch)); 131 | 132 | std::cout << "x = " << T(0, 3) << " " << "y = " << T(1, 3) << " " << "z = " << T(2, 3) << std::endl; 133 | std::cout << "Roll = " << roll << " " << "Pitch = " << pitch << " " << "Yaw = " << yaw << " " << std::endl; 134 | 135 | } 136 | 137 | int main(int argc, char **argv) { 138 | ros::init(argc, argv, "control_robot_vel"); 139 | ros::NodeHandle n; 140 | ros::AsyncSpinner spinner(1); 141 | ROS_INFO_STREAM("start"); 142 | ros::Publisher spd_pub = n.advertise("/probot_anno/arm_vel_controller/command", 100); 143 | std_msgs::Float64MultiArray spd_msg; 144 | spd_msg.data.push_back(0.0); 145 | spd_msg.data.push_back(0.0); 146 | spd_msg.data.push_back(0.0); 147 | spd_msg.data.push_back(0.0); 148 | spd_msg.data.push_back(0.0); 149 | spd_msg.data.push_back(0.0); 150 | ros::Rate loop_rate(50); 151 | Eigen::MatrixXd myDHparam(6, 4); 152 | 153 | myDHparam << 0, 0, 0.284, 0, 154 | pi / 2, 0, 0, pi / 2, 155 | 0, 0.225, 0, 0 , 156 | pi / 2, 0, 0.2289, 0 , 157 | -pi / 2, 0, 0, -pi / 2 , 158 | pi / 2, 0, 0.055, 0; 159 | Anno_robot myrobot(myDHparam); 160 | double time = 0.02; 161 | Eigen::VectorXd V = Eigen::VectorXd::Zero(6,1); 162 | 163 | while (time <= 12) { 164 | if (time >= 0 && time <= 4) { 165 | V(0) = 0.003535 * time; 166 | } 167 | else if (time > 4 && time <= 7) { 168 | V(0) = 0.01414; 169 | } 170 | else if (time > 7 && time <= 12) { 171 | V(0) = 0.033936 - 0.002828 * time; 172 | } 173 | V(2) = -V(0); 174 | 175 | //try { 176 | myrobot.setEndVelocity(V); 177 | 178 | std::cout<<"joint velocity:"<(i, 0) = pathInterpolation3(0, end_joint_state(i), 0, 0, 10).transpose(); 34 | } 35 | 36 | //节点初始化 37 | ros::init(argc, argv, "traj"); 38 | //创建节点句柄对象 39 | ros::NodeHandle n; 40 | ros::AsyncSpinner spinner(1); 41 | ROS_INFO_STREAM("start"); 42 | //创建发布者对象，用于发布位置信息 43 | ros::Publisher spd_pub = n.advertise("/probot_anno/arm_vel_controller/command", 1000); 44 | //设置信息发送频率（50Hz） 45 | ros::Rate loop_rate(50); 46 | 47 | //初始化publish的变量并初始化6个值 48 | std_msgs::Float64MultiArray spd_msg; 49 | spd_msg.data.push_back(0); 50 | spd_msg.data.push_back(0); 51 | spd_msg.data.push_back(0); 52 | spd_msg.data.push_back(0); 53 | spd_msg.data.push_back(0); 54 | spd_msg.data.push_back(0); 55 | 56 | double t = 0.0; 57 | Eigen::VectorXd v(6); 58 | while (ros::ok()) 59 | { 60 | v = calcVel (joint_state_interp_param, t); 61 | if(t >= 10) break; 62 | //为要发送的变量装入解出的六关节坐标 63 | spd_msg.data.at(0) = v(0); 64 | spd_msg.data.at(1) = v(1); 65 | spd_msg.data.at(2) = v(2); 66 | spd_msg.data.at(3) = v(3); 67 | spd_msg.data.at(4) = v(4); 68 | spd_msg.data.at(5) = v(5); 69 | 70 | //发送出去，若成功，机械臂状态会改变 71 | spd_pub.publish(spd_msg); 72 | cout << v << endl; 73 | 74 | ROS_INFO_STREAM("published"); 75 | 76 | t = t + 0.02; 77 | loop_rate.sleep(); 78 | cout << "t: " << t << endl; 79 | } 80 | 81 | return 0; 82 | } 83 | 84 | //Interpolation: p = a0 + a1 * t + a2 * t^2 + a3 * t^3 85 | //arg: init_p, final_p, init_v, final_v, period 86 | //return: [a0, a1, a2, a3] 87 | Eigen::VectorXd pathInterpolation3(double init_p, double final_p, double init_v, double final_v, double period) 88 | { 89 | Eigen::VectorXd param(4); 90 | 91 | param(0) = init_p; 92 | param(1) = init_v; 93 | param(2) = 3 * (final_p - init_p) / pow(period, 2) - 2 * init_v / period - final_v / period; 94 | param(3) = -2 * (final_p - init_p) / pow(period, 3) + (final_v + init_v) / pow(period, 2); 95 | 96 | return param; 97 | } 98 | 99 | Eigen::VectorXd calcVel(Eigen::MatrixXd theta_t, double t){ 100 | Eigen::VectorXd v(6); 101 | for (int i = 0; i < 6; i++) { 102 | v(i) = theta_t(i, 1) + 2 * theta_t(i, 2) * t + 3 * theta_t(i, 3) * t * t; 103 | } 104 | return v; 105 | } -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/src/trajwithobstacle.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "vector" 5 | #include 6 | #include 7 | 8 | #include 9 | #include "std_msgs/Float32.h" 10 | #include "std_msgs/Float64MultiArray.h" 11 | #include "std_msgs/Float32MultiArray.h" 12 | #include "controller_manager_msgs/SwitchController.h" 13 | #include "controller_manager_msgs/ListControllers.h" 14 | #include "probot_anno_manipulator_ikfast_moveit_plugin.cpp" 15 | 16 | #define pi 3.1415926535897932384626 17 | 18 | using namespace Eigen; 19 | using namespace std; 20 | 21 | VectorXd pathInterpolation3(double init_p, double final_p, double init_v, double final_v, double period); 22 | MatrixXd pathInterp3WithMidpoint(double init_p, double mid_p, double final_p, double whole_period); 23 | VectorXd calcVel(MatrixXd theta_t, double t); 24 | 25 | int main(int argc, char** argv) 26 | { 27 | //节点初始化 28 | ros::init(argc, argv, "control_example"); 29 | //创建节点句柄对象 30 | ros::NodeHandle n; 31 | ros::AsyncSpinner spinner(1); 32 | ROS_INFO_STREAM("start"); 33 | //创建发布者对象，用于发布位置信息 34 | ros::Publisher spd_pub = n.advertise("/probot_anno/arm_vel_controller/command", 1000); 35 | //设置信息发送频率（50Hz） 36 | ros::Rate loop_rate(50); 37 | 38 | //初始化publish的变量并初始化6个值 39 | std_msgs::Float64MultiArray spd_msg; 40 | spd_msg.data.push_back(0.0); 41 | spd_msg.data.push_back(0.0); 42 | spd_msg.data.push_back(0.0); 43 | spd_msg.data.push_back(0.0); 44 | spd_msg.data.push_back(0.0); 45 | spd_msg.data.push_back(0.0); 46 | 47 | //blue ball 0.26 0.15 0.08 48 | VectorXd blueball_joint_state(6); 49 | blueball_joint_state << 0.523405, -1.28345, 0.0903509, -0.00186839, 1.1927, 0.525141; 50 | 51 | //red ball 0.28 -0.24 0.08 52 | VectorXd redball_joint_state(6); 53 | redball_joint_state << -0.708536, -1.44723, 0.565107, -0.000952057, 0.882642, -0.707801; 54 | 55 | 56 | /******************************adjust the three parameters to control speed ****************************/ 57 | //middle point, which is supposed to be calculated by inverse kinematics, while replaced with empirical numbers 58 | VectorXd midpoint_joint_state(6); 59 | midpoint_joint_state << 0, -1.2, 0.45, 0, 0.8, 0; // further from the obstacle 60 | //midpoint_joint_state << 0, -1.4, 0.6, 0, 0.8, 0; // closer to the obstacle 61 | //midpoint_joint_state << 0.000115258, -1.15935, 0.450616, -0.00104892, 0.708735, 0.000797974; 62 | 63 | double timeperiod_s2b = 5; // the time spent from start point to reaching the blue ball for the first time 64 | double timeperiod_r2b = 6; // the time spent from red ball to blue ball (same time spent on blue to red) 65 | /******************************adjust the three parameters to control speed****************************/ 66 | 67 | MatrixXd start2blue_joint_state_interp_param(6,4); // the coeffients of the joint state interpolation polynomial from start to blue ball 68 | MatrixXd blue2mid_joint_state_interp_param(6,4); 69 | MatrixXd mid2red_joint_state_interp_param(6,4); 70 | MatrixXd red2mid_joint_state_interp_param(6,4); 71 | MatrixXd mid2blue_joint_state_interp_param(6,4); 72 | 73 | // calculate the coefficients of the interpolation polynomials 74 | for (int i = 0; i < 6; i++){ 75 | start2blue_joint_state_interp_param.block <1, 4>(i, 0) = pathInterpolation3(0, blueball_joint_state(i), 0, 0, timeperiod_s2b).transpose(); 76 | blue2mid_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(blueball_joint_state(i), midpoint_joint_state(i), redball_joint_state(i), timeperiod_r2b).block <1,4>(0,0); 77 | mid2red_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(blueball_joint_state(i), midpoint_joint_state(i), redball_joint_state(i), timeperiod_r2b).block <1,4>(1,0); 78 | red2mid_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(redball_joint_state(i), midpoint_joint_state(i), blueball_joint_state(i), timeperiod_r2b).block <1,4>(0,0); 79 | mid2blue_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(redball_joint_state(i), midpoint_joint_state(i), blueball_joint_state(i), timeperiod_r2b).block <1,4>(1,0); 80 | } 81 | 82 | //from start point to reaching blue ball for the first time 83 | double t = 0.0; 84 | const double timestep = 0.02; 85 | VectorXd vel(6); 86 | while (ros::ok()) 87 | { 88 | if (t >= timeperiod_s2b) break; 89 | 90 | vel = calcVel(start2blue_joint_state_interp_param, t); 91 | 92 | spd_msg.data.at(0) = vel(0); 93 | spd_msg.data.at(1) = vel(1); 94 | spd_msg.data.at(2) = vel(2); 95 | spd_msg.data.at(3) = vel(3); 96 | spd_msg.data.at(4) = vel(4); 97 | spd_msg.data.at(5) = vel(5); 98 | spd_pub.publish(spd_msg); 99 | ROS_INFO_STREAM("published"); 100 | 101 | t = t + timestep; 102 | loop_rate.sleep(); 103 | } 104 | 105 | //back and forth between blue and red ball 106 | int current_state = 0; // 0: blue2mid, 1: mid2red, 2: red2mid, 3: mid2blue 107 | while (ros::ok()){ 108 | for(t = 0; t <= timeperiod_r2b / 2; t += timestep, loop_rate.sleep()){ 109 | switch(current_state){ 110 | case 0: 111 | vel = calcVel (blue2mid_joint_state_interp_param, t); 112 | break; 113 | case 1: 114 | vel = calcVel (mid2red_joint_state_interp_param, t); 115 | break; 116 | case 2: 117 | vel = calcVel (red2mid_joint_state_interp_param, t); 118 | break; 119 | case 3: 120 | vel = calcVel (mid2blue_joint_state_interp_param, t); 121 | break; 122 | } 123 | spd_msg.data.at(0) = vel(0); 124 | spd_msg.data.at(1) = vel(1); 125 | spd_msg.data.at(2) = vel(2); 126 | spd_msg.data.at(3) = vel(3); 127 | spd_msg.data.at(4) = vel(4); 128 | spd_msg.data.at(5) = vel(5); 129 | spd_pub.publish(spd_msg); 130 | ROS_INFO_STREAM("published"); 131 | } 132 | current_state += 1; 133 | if(current_state == 4) current_state = 0; 134 | } 135 | return 0; 136 | } 137 | 138 | 139 | //Interpolation: p = a0 + a1 * t + a2 * t^2 + a3 * t^3 140 | //arg: init_p, final_p, init_v, final_v, period 141 | //return: [a0, a1, a2, a3] 142 | VectorXd pathInterpolation3(double init_p, double final_p, double init_v, double final_v, double period){ 143 | VectorXd param(4); 144 | 145 | param(0) = init_p; 146 | param(1) = init_v; 147 | param(2) = 3 * (final_p - init_p) / pow(period, 2) - 2 * init_v / period - final_v / period; 148 | param(3) = -2 * (final_p - init_p) / pow(period, 3) + (final_v + init_v) / pow(period, 2); 149 | 150 | return param; 151 | } 152 | 153 | //Interpolation with midpoint: p = a0 + a1 * t + a2 * t^2 + a3 * t^3 154 | //arg: init_p, final_p, init_v, final_v, whole_period 155 | //return: [a10, a11, a12, a13 156 | // a20, a21, a22, a23] 157 | MatrixXd pathInterp3WithMidpoint(double init_p, double mid_p, double final_p, double whole_period){ 158 | //tf1 = tf2 159 | double tf = whole_period / 2; 160 | MatrixXd A(2, 4); 161 | 162 | A(0, 0) = init_p; 163 | A(0, 1) = 0; 164 | A(0, 2) = (12 * mid_p - 3 * final_p - 9 * init_p) / (4 * tf * tf); 165 | A(0, 3) = (-8 * mid_p + 3 * final_p + 5 * init_p) / (4 * tf * tf * tf); 166 | A(1, 0) = mid_p; 167 | A(1, 1) = (3 * final_p - 3 * init_p) / (4 * tf); 168 | A(1, 2) = (-12 * mid_p + 6 * final_p + 6 * init_p) / (4 * tf * tf); 169 | A(1, 3) = (8 * mid_p - 5 * final_p - 3 * init_p) / (4 * tf * tf * tf); 170 | 171 | return A; 172 | } 173 | 174 | VectorXd calcVel(MatrixXd theta_t, double t){ 175 | VectorXd v(6); 176 | for (int i = 0; i < 6; i++) { 177 | v(i) = theta_t(i, 1) + 2 * theta_t(i, 2) * t + 3 * theta_t(i, 3) * t * t; 178 | } 179 | return v; 180 | } -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/worlds/tableObj.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | 11 | model://ground_plane 12 | 13 | 14 | 15 | 16 | 0.001 17 | 1 18 | 1000 19 | 0 0 -9.81 20 | 21 | 22 | 23 | 24 | 0.30 0.0 0.2165 0 0 0 25 | 26 | 27 | 0.001 28 | 29 | 0.00016 30 | 0 31 | 0 32 | 0.00016 33 | 0 34 | 0.00006 35 | 36 | 37 | 38 | 39 | 40 | 44 | 47 | 48 | model://coke_can/meshes/coke_can.dae 49 | 0.2 0.15 0.18 50 | 51 | 52 | 10 53 | 54 | 55 | 56 | 100.0 57 | 100.0 58 | 59 | 60 | 61 | 62 | 10000000.0 63 | 1.0 64 | 0.001 65 | 0.1 66 | 67 | 68 | 69 | 70 | 71 | 72 | 76 | 79 | 80 | model://coke_can/meshes/coke_can.dae 81 | 0.2 0.15 0.18 82 | 83 | 84 | 85 | 89 | 90 | 91 | 0 92 | 0 93 | 94 | 95 | 96 | 97 | true 98 | 0.40 0.0 0.1 0 -0 1.5707963265 99 | 100 | 101 | 1 102 | 103 | 0.166667 104 | 0 105 | 0 106 | 0.166667 107 | 0 108 | 0.166667 109 | 110 | 111 | 112 | 113 | 114 | 0.30 0.30 0.2 115 | 116 | 117 | 10 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 0.6 126 | 0.6 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 0.30 0.30 0.2 138 | 139 | 140 | 141 | 145 | 146 | 147 | 0 148 | 0 149 | 150 | 151 | 152 | 153 | 154 | 155 | -------------------------------------------------------------------------------- /robot_arm/probot_gazebo/worlds/tableObj2.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 11 | 12 | model://sun 13 | 14 | 15 | 16 | 17 | 0.0 0.0 0.0 1.0 18 | 0 19 | 20 | 21 | 22 | 23 | 24 | 1 25 | 26 | 27 | 28 | 29 | 0 0 1 30 | 100 100 31 | 32 | 33 | 34 | 35 | 36 | 100 37 | 50 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 10 46 | 47 | 48 | 0 49 | 50 | 51 | 0 0 1 52 | 100 100 53 | 54 | 55 | 56 | 60 | 61 | 62 | 63 | 0 64 | 0 65 | 66 | 0 67 | 0 68 | 1 69 | 70 | 71 | 72 | 1 73 | 0 0 10 0 -0 0 74 | 0.8 0.8 0.8 1 75 | 0.2 0.2 0.2 1 76 | 77 | 1000 78 | 0.9 79 | 0.01 80 | 0.001 81 | 82 | -0.5 0.1 -0.9 83 | 84 | 85 | 0 0 0 1 86 | 0 87 | 0.7 0.7 0.7 1 88 | 89 | 90 | 0.001 91 | 1 92 | 1000 93 | 0 0 -9.81 94 | 95 | 96 | EARTH_WGS84 97 | 0 98 | 0 99 | 0 100 | 0 101 | 102 | 103 | 104 | 105 | 0 0 0 0 0 0 106 | 107 | 0.0565 108 | 109 | 2.3541666666666672e-05 110 | 0 111 | 0 112 | 2.3541666666666672e-05 113 | 0 114 | 2.3541666666666672e-05 115 | 116 | 117 | 118 | 0 0 0 0 0 0 119 | 120 | 121 | 0.05 0.02 0.05 122 | 123 | 124 | 125 | 126 | 127 | 100000 128 | 1 129 | 1.0 130 | 0.001 131 | 1 132 | 1 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 10 141 | 142 | 143 | 144 | 0 0 0 0 0 0 145 | 146 | 147 | 0.05 0.02 0.05 148 | 149 | 150 | 151 | 155 | 156 | 157 | 158 | 0 159 | 0 160 | 161 | 1 162 | 163 | 0 164 | 0 165 | 166 | 0 167 | 0 168 | 169 | 0.23 0.0 0.0 0 0 0 170 | 0 171 | 172 | 173 | 174 | 30 70000000 175 | 32 225166473 176 | 1479971495 804440900 177 | 178 | 0 0 0 0 -0 0 179 | 180 | 0 0 0 0 -0 0 181 | 0 0 0 0 -0 0 182 | 0 0 0 0 -0 0 183 | 0 0 0 0 -0 0 184 | 185 | 186 | 187 | 188 | 189 | 190 | 3.46609 -3.32345 1.61365 0 0.275643 2.35619 191 | orbit 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | -------------------------------------------------------------------------------- /robot_arm/实验1：正逆运动学/DK.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | using namespace std; 8 | using namespace Eigen; 9 | 10 | #define pi 3.1415926535897932384626 11 | 12 | class DK{ 13 | public: 14 | Eigen::MatrixXd calDK(); 15 | Eigen::MatrixXd calT(double alpha, double a, double d, double w); 16 | double w[6]{0}; 17 | 18 | private: 19 | 20 | }; 21 | 22 | Eigen::MatrixXd DK::calT(double alpha, double a, double d, double w){ 23 | Eigen::MatrixXd T = Eigen::MatrixXd::Ones(4,4); 24 | 25 | T(0,0) = std::cos(w); 26 | T(0,1) = -std::sin(w); 27 | T(0,2) = 0; 28 | T(0,3) = a; 29 | T(1,0) = std::sin(w)*std::cos(alpha); 30 | T(1,1) = std::cos(w)*std::cos(alpha); 31 | T(1,2) = -std::sin(alpha); 32 | T(1,3) = -std::sin(alpha)*d; 33 | T(2,0) = std::sin(w)*std::sin(alpha); 34 | T(2,1) = std::cos(w)*std::sin(alpha); 35 | T(2,2) = std::cos(alpha); 36 | T(2,3) = cos(alpha)*d; 37 | T(3,0) = 0; 38 | T(3,1) = 0; 39 | T(3,2) = 0; 40 | T(3,3) = 1; 41 | 42 | return T; 43 | 44 | } 45 | 46 | Eigen::MatrixXd DK::calDK() 47 | { 48 | Eigen::MatrixXd T = Eigen::MatrixXd::Identity(4,4); 49 | 50 | T = T * calT(0,0,0.284,this->w[0]); 51 | T = T * calT(pi/2,0,0,pi/2+this->w[1]); 52 | T = T * calT(0,0.225,0,this->w[2]); 53 | T = T * calT(pi/2,0,0.2289,this->w[3]); 54 | T = T * calT(-pi/2,0,0,-pi/2+this->w[4]); 55 | T = T * calT(pi/2,0,0.055,this->w[5]); 56 | T = T * calT(-pi/2,0,0,0); 57 | 58 | return T; 59 | } 60 | int main(int argc, char **argv) 61 | { 62 | DK dk; 63 | for (int i=0;i<6;i++) 64 | { 65 | std::cin>>dk.w[i]; 66 | } 67 | 68 | Eigen::MatrixXd T = dk.calDK(); 69 | Eigen::MatrixXd xyz = T.block<3,1>(0,3).transpose(); 70 | double pitch = asin(-T(2,0)); 71 | double yaw = asin(T(1,0)/std::cos(pitch)); 72 | double roll = acos(T(2,2)/std::cos(pitch)); 73 | 74 | std::cout<<"x,y,z: "< 2 | #include 3 | #include 4 | #include 5 | #include "vector" 6 | #include "std_msgs/Float32.h" 7 | #include "std_msgs/Float64MultiArray.h" 8 | #include "controller_manager_msgs/SwitchController.h" 9 | #include "controller_manager_msgs/ListControllers.h" 10 | #include "probot_anno_manipulator_ikfast_moveit_plugin.cpp" 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #define pi 3.1415926535897932384626 18 | using namespace std; 19 | class Anno_robot{ 20 | public: 21 | Anno_robot(Eigen::MatrixXd DHparam); 22 | ~Anno_robot() {}; 23 | Eigen::VectorXd getJointState(); 24 | void setEndVelocity(Eigen::VectorXd); 25 | Eigen::MatrixXd getJacobian(); 26 | Eigen::MatrixXd calcTransform(double a, double b, double d, double o); 27 | void printParam(); 28 | static double timestep; 29 | Eigen::VectorXd joint_velocity; 30 | 31 | private: 32 | Eigen::MatrixXd DHparam; 33 | Eigen::VectorXd end_velocity; 34 | 35 | }; 36 | 37 | double Anno_robot::timestep = 0.02; 38 | 39 | Anno_robot::Anno_robot(Eigen::MatrixXd DHparam) { 40 | this->DHparam = DHparam; 41 | end_velocity = Eigen::VectorXd::Zero(6,1); 42 | joint_velocity = Eigen::VectorXd::Zero(6,1); 43 | } 44 | 45 | Eigen::VectorXd Anno_robot::getJointState() { 46 | return DHparam.block<6, 1>(0, 3); 47 | } 48 | 49 | void Anno_robot::setEndVelocity(Eigen::VectorXd V) { 50 | this->end_velocity = V; 51 | Eigen::MatrixXd J = this->getJacobian(); 52 | 53 | if (J.determinant() != 0) { 54 | this->joint_velocity = J.inverse() * V; 55 | } 56 | else { 57 | std::cout << "�ſɱȾ������죡�޷������ٶ�" << std::endl; 58 | //try { 59 | //throw 0; 60 | //} 61 | Eigen::VectorXd delta; 62 | } 63 | 64 | Eigen::VectorXd delta_joint_state = Anno_robot::timestep * this->joint_velocity; 65 | DHparam.block<6, 1>(0, 3) += delta_joint_state; 66 | 67 | } 68 | 69 | Eigen::MatrixXd Anno_robot::getJacobian(){ 70 | Eigen::MatrixXd P(3, 6); 71 | Eigen::MatrixXd Z(3, 6); 72 | Eigen::MatrixXd T = Eigen::MatrixXd::Identity(4, 4); 73 | 74 | for (int i = 0; i < 6; i++) { 75 | T = T * calcTransform(DHparam(i,0), DHparam(i,1), DHparam(i,2), DHparam(i,3)); 76 | P.block<3, 1>(0, i) = T.block<3, 1>(0, 3); 77 | Z.block<3, 1>(0, i) = T.block<3, 1>(0, 2); 78 | 79 | } 80 | 81 | Eigen::MatrixXd J(6, 6); 82 | Eigen::Vector3d z(3); 83 | Eigen::Vector3d p(3); 84 | Eigen::Vector3d pN = P.block<3, 1>(0, 5); 85 | 86 | for (int i = 0; i < 5; i++){ 87 | z = Z.block<3, 1>(0, i); 88 | p = P.block<3, 1>(0, i); 89 | J.block<3, 1>(0, i) = z.cross(pN - p); 90 | J.block<3, 1>(3, i) = z; 91 | } 92 | J.block<3, 1>(0, 5) = Eigen::VectorXd::Zero(3, 1); 93 | J.block<3, 1>(3, 5) = Z.block<3, 1>(0, 5); 94 | 95 | return J; 96 | } 97 | 98 | Eigen::MatrixXd Anno_robot::calcTransform(double b, double a, double d, double o) 99 | { 100 | Eigen::MatrixXd T(4, 4); // T[i-1 -> i] 101 | T << cos(o), -sin(o), 0, a, 102 | sin(o)* cos(b), cos(o)* cos(b), -sin(b), -sin(b) * d, 103 | sin(o)* sin(b), cos(o)* sin(b), cos(b), cos(b)* d, 104 | 0, 0, 0, 1; 105 | return T; 106 | } 107 | 108 | void Anno_robot::printParam() 109 | { 110 | 111 | Eigen::MatrixXd T = Eigen::MatrixXd::Identity(4,4); 112 | 113 | for (int i = 0; i < 6; i++) { 114 | T = T * calcTransform(DHparam(i, 0), DHparam(i, 1), DHparam(i, 2), DHparam(i, 3)); 115 | } 116 | 117 | Eigen::MatrixXd Final(4, 4); 118 | Final << 1, 0, 0, 0, 119 | 0, 0, 1, 0, 120 | 0, -1, 0, 0, 121 | 0, 0, 0, 1; 122 | T = T * Final; 123 | 124 | std::cout << "transformation matrix T:" << std::endl; 125 | std::cout << T << std::endl; 126 | 127 | double yaw, pitch, roll; 128 | pitch = -asin(T(2, 0)); 129 | yaw = asin(T(1, 0) / cos(pitch)); 130 | roll = asin(T(2, 1) / cos(pitch)); 131 | 132 | std::cout << "x = " << T(0, 3) << " " << "y = " << T(1, 3) << " " << "z = " << T(2, 3) << std::endl; 133 | std::cout << "Roll = " << roll << " " << "Pitch = " << pitch << " " << "Yaw = " << yaw << " " << std::endl; 134 | 135 | } 136 | 137 | int main(int argc, char **argv) { 138 | ros::init(argc, argv, "control_robot_vel"); 139 | ros::NodeHandle n; 140 | ros::AsyncSpinner spinner(1); 141 | ROS_INFO_STREAM("start"); 142 | ros::Publisher spd_pub = n.advertise("/probot_anno/arm_vel_controller/command", 100); 143 | std_msgs::Float64MultiArray spd_msg; 144 | spd_msg.data.push_back(0.0); 145 | spd_msg.data.push_back(0.0); 146 | spd_msg.data.push_back(0.0); 147 | spd_msg.data.push_back(0.0); 148 | spd_msg.data.push_back(0.0); 149 | spd_msg.data.push_back(0.0); 150 | ros::Rate loop_rate(50); 151 | Eigen::MatrixXd myDHparam(6, 4); 152 | 153 | myDHparam << 0, 0, 0.284, 0, 154 | pi / 2, 0, 0, pi / 2, 155 | 0, 0.225, 0, 0 , 156 | pi / 2, 0, 0.2289, 0 , 157 | -pi / 2, 0, 0, -pi / 2 , 158 | pi / 2, 0, 0.055, 0; 159 | Anno_robot myrobot(myDHparam); 160 | double time = 0.02; 161 | Eigen::VectorXd V = Eigen::VectorXd::Zero(6,1); 162 | 163 | while (time <= 12) { 164 | if (time >= 0 && time <= 4) { 165 | V(0) = 0.003535 * time; 166 | } 167 | else if (time > 4 && time <= 7) { 168 | V(0) = 0.01414; 169 | } 170 | else if (time > 7 && time <= 12) { 171 | V(0) = 0.033936 - 0.002828 * time; 172 | } 173 | V(2) = -V(0); 174 | 175 | //try { 176 | myrobot.setEndVelocity(V); 177 | 178 | std::cout<<"joint velocity:"<(i, 0) = pathInterpolation3(0, end_joint_state(i), 0, 0, 10).transpose(); 34 | } 35 | 36 | //节点初始化 37 | ros::init(argc, argv, "traj"); 38 | //创建节点句柄对象 39 | ros::NodeHandle n; 40 | ros::AsyncSpinner spinner(1); 41 | ROS_INFO_STREAM("start"); 42 | //创建发布者对象，用于发布位置信息 43 | ros::Publisher spd_pub = n.advertise("/probot_anno/arm_vel_controller/command", 1000); 44 | //设置信息发送频率（50Hz） 45 | ros::Rate loop_rate(50); 46 | 47 | //初始化publish的变量并初始化6个值 48 | std_msgs::Float64MultiArray spd_msg; 49 | spd_msg.data.push_back(0); 50 | spd_msg.data.push_back(0); 51 | spd_msg.data.push_back(0); 52 | spd_msg.data.push_back(0); 53 | spd_msg.data.push_back(0); 54 | spd_msg.data.push_back(0); 55 | 56 | double t = 0.0; 57 | Eigen::VectorXd v(6); 58 | while (ros::ok()) 59 | { 60 | v = calcVel (joint_state_interp_param, t); 61 | if(t >= 10) break; 62 | //为要发送的变量装入解出的六关节坐标 63 | spd_msg.data.at(0) = v(0); 64 | spd_msg.data.at(1) = v(1); 65 | spd_msg.data.at(2) = v(2); 66 | spd_msg.data.at(3) = v(3); 67 | spd_msg.data.at(4) = v(4); 68 | spd_msg.data.at(5) = v(5); 69 | 70 | //发送出去，若成功，机械臂状态会改变 71 | spd_pub.publish(spd_msg); 72 | cout << v << endl; 73 | 74 | ROS_INFO_STREAM("published"); 75 | 76 | t = t + 0.02; 77 | loop_rate.sleep(); 78 | cout << "t: " << t << endl; 79 | } 80 | 81 | return 0; 82 | } 83 | 84 | //Interpolation: p = a0 + a1 * t + a2 * t^2 + a3 * t^3 85 | //arg: init_p, final_p, init_v, final_v, period 86 | //return: [a0, a1, a2, a3] 87 | Eigen::VectorXd pathInterpolation3(double init_p, double final_p, double init_v, double final_v, double period) 88 | { 89 | Eigen::VectorXd param(4); 90 | 91 | param(0) = init_p; 92 | param(1) = init_v; 93 | param(2) = 3 * (final_p - init_p) / pow(period, 2) - 2 * init_v / period - final_v / period; 94 | param(3) = -2 * (final_p - init_p) / pow(period, 3) + (final_v + init_v) / pow(period, 2); 95 | 96 | return param; 97 | } 98 | 99 | Eigen::VectorXd calcVel(Eigen::MatrixXd theta_t, double t){ 100 | Eigen::VectorXd v(6); 101 | for (int i = 0; i < 6; i++) { 102 | v(i) = theta_t(i, 1) + 2 * theta_t(i, 2) * t + 3 * theta_t(i, 3) * t * t; 103 | } 104 | return v; 105 | } -------------------------------------------------------------------------------- /robot_arm/实验4：敲铃实验（仿真）/trajwithobstacle.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "vector" 5 | #include 6 | #include 7 | 8 | #include 9 | #include "std_msgs/Float32.h" 10 | #include "std_msgs/Float64MultiArray.h" 11 | #include "std_msgs/Float32MultiArray.h" 12 | #include "controller_manager_msgs/SwitchController.h" 13 | #include "controller_manager_msgs/ListControllers.h" 14 | #include "probot_anno_manipulator_ikfast_moveit_plugin.cpp" 15 | 16 | #define pi 3.1415926535897932384626 17 | 18 | using namespace Eigen; 19 | using namespace std; 20 | 21 | VectorXd pathInterpolation3(double init_p, double final_p, double init_v, double final_v, double period); 22 | MatrixXd pathInterp3WithMidpoint(double init_p, double mid_p, double final_p, double whole_period); 23 | VectorXd calcVel(MatrixXd theta_t, double t); 24 | 25 | int main(int argc, char** argv) 26 | { 27 | //节点初始化 28 | ros::init(argc, argv, "control_example"); 29 | //创建节点句柄对象 30 | ros::NodeHandle n; 31 | ros::AsyncSpinner spinner(1); 32 | ROS_INFO_STREAM("start"); 33 | //创建发布者对象，用于发布位置信息 34 | ros::Publisher spd_pub = n.advertise("/probot_anno/arm_vel_controller/command", 1000); 35 | //设置信息发送频率（50Hz） 36 | ros::Rate loop_rate(50); 37 | 38 | //初始化publish的变量并初始化6个值 39 | std_msgs::Float64MultiArray spd_msg; 40 | spd_msg.data.push_back(0.0); 41 | spd_msg.data.push_back(0.0); 42 | spd_msg.data.push_back(0.0); 43 | spd_msg.data.push_back(0.0); 44 | spd_msg.data.push_back(0.0); 45 | spd_msg.data.push_back(0.0); 46 | 47 | //blue ball 0.26 0.15 0.08 48 | VectorXd blueball_joint_state(6); 49 | blueball_joint_state << 0.523405, -1.28345, 0.0903509, -0.00186839, 1.1927, 0.525141; 50 | 51 | //red ball 0.28 -0.24 0.08 52 | VectorXd redball_joint_state(6); 53 | redball_joint_state << -0.708536, -1.44723, 0.565107, -0.000952057, 0.882642, -0.707801; 54 | 55 | 56 | /******************************adjust the three parameters to control speed ****************************/ 57 | //middle point, which is supposed to be calculated by inverse kinematics, while replaced with empirical numbers 58 | VectorXd midpoint_joint_state(6); 59 | midpoint_joint_state << 0, -1.2, 0.45, 0, 0.8, 0; // further from the obstacle 60 | //midpoint_joint_state << 0, -1.4, 0.6, 0, 0.8, 0; // closer to the obstacle 61 | //midpoint_joint_state << 0.000115258, -1.15935, 0.450616, -0.00104892, 0.708735, 0.000797974; 62 | 63 | double timeperiod_s2b = 5; // the time spent from start point to reaching the blue ball for the first time 64 | double timeperiod_r2b = 6; // the time spent from red ball to blue ball (same time spent on blue to red) 65 | /******************************adjust the three parameters to control speed****************************/ 66 | 67 | MatrixXd start2blue_joint_state_interp_param(6,4); // the coeffients of the joint state interpolation polynomial from start to blue ball 68 | MatrixXd blue2mid_joint_state_interp_param(6,4); 69 | MatrixXd mid2red_joint_state_interp_param(6,4); 70 | MatrixXd red2mid_joint_state_interp_param(6,4); 71 | MatrixXd mid2blue_joint_state_interp_param(6,4); 72 | 73 | // calculate the coefficients of the interpolation polynomials 74 | for (int i = 0; i < 6; i++){ 75 | start2blue_joint_state_interp_param.block <1, 4>(i, 0) = pathInterpolation3(0, blueball_joint_state(i), 0, 0, timeperiod_s2b).transpose(); 76 | blue2mid_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(blueball_joint_state(i), midpoint_joint_state(i), redball_joint_state(i), timeperiod_r2b).block <1,4>(0,0); 77 | mid2red_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(blueball_joint_state(i), midpoint_joint_state(i), redball_joint_state(i), timeperiod_r2b).block <1,4>(1,0); 78 | red2mid_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(redball_joint_state(i), midpoint_joint_state(i), blueball_joint_state(i), timeperiod_r2b).block <1,4>(0,0); 79 | mid2blue_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(redball_joint_state(i), midpoint_joint_state(i), blueball_joint_state(i), timeperiod_r2b).block <1,4>(1,0); 80 | } 81 | 82 | //from start point to reaching blue ball for the first time 83 | double t = 0.0; 84 | const double timestep = 0.02; 85 | VectorXd vel(6); 86 | while (ros::ok()) 87 | { 88 | if (t >= timeperiod_s2b) break; 89 | 90 | vel = calcVel(start2blue_joint_state_interp_param, t); 91 | 92 | spd_msg.data.at(0) = vel(0); 93 | spd_msg.data.at(1) = vel(1); 94 | spd_msg.data.at(2) = vel(2); 95 | spd_msg.data.at(3) = vel(3); 96 | spd_msg.data.at(4) = vel(4); 97 | spd_msg.data.at(5) = vel(5); 98 | spd_pub.publish(spd_msg); 99 | ROS_INFO_STREAM("published"); 100 | 101 | t = t + timestep; 102 | loop_rate.sleep(); 103 | } 104 | 105 | //back and forth between blue and red ball 106 | int current_state = 0; // 0: blue2mid, 1: mid2red, 2: red2mid, 3: mid2blue 107 | while (ros::ok()){ 108 | for(t = 0; t <= timeperiod_r2b / 2; t += timestep, loop_rate.sleep()){ 109 | switch(current_state){ 110 | case 0: 111 | vel = calcVel (blue2mid_joint_state_interp_param, t); 112 | break; 113 | case 1: 114 | vel = calcVel (mid2red_joint_state_interp_param, t); 115 | break; 116 | case 2: 117 | vel = calcVel (red2mid_joint_state_interp_param, t); 118 | break; 119 | case 3: 120 | vel = calcVel (mid2blue_joint_state_interp_param, t); 121 | break; 122 | } 123 | spd_msg.data.at(0) = vel(0); 124 | spd_msg.data.at(1) = vel(1); 125 | spd_msg.data.at(2) = vel(2); 126 | spd_msg.data.at(3) = vel(3); 127 | spd_msg.data.at(4) = vel(4); 128 | spd_msg.data.at(5) = vel(5); 129 | spd_pub.publish(spd_msg); 130 | ROS_INFO_STREAM("published"); 131 | } 132 | current_state += 1; 133 | if(current_state == 4) current_state = 0; 134 | } 135 | return 0; 136 | } 137 | 138 | 139 | //Interpolation: p = a0 + a1 * t + a2 * t^2 + a3 * t^3 140 | //arg: init_p, final_p, init_v, final_v, period 141 | //return: [a0, a1, a2, a3] 142 | VectorXd pathInterpolation3(double init_p, double final_p, double init_v, double final_v, double period){ 143 | VectorXd param(4); 144 | 145 | param(0) = init_p; 146 | param(1) = init_v; 147 | param(2) = 3 * (final_p - init_p) / pow(period, 2) - 2 * init_v / period - final_v / period; 148 | param(3) = -2 * (final_p - init_p) / pow(period, 3) + (final_v + init_v) / pow(period, 2); 149 | 150 | return param; 151 | } 152 | 153 | //Interpolation with midpoint: p = a0 + a1 * t + a2 * t^2 + a3 * t^3 154 | //arg: init_p, final_p, init_v, final_v, whole_period 155 | //return: [a10, a11, a12, a13 156 | // a20, a21, a22, a23] 157 | MatrixXd pathInterp3WithMidpoint(double init_p, double mid_p, double final_p, double whole_period){ 158 | //tf1 = tf2 159 | double tf = whole_period / 2; 160 | MatrixXd A(2, 4); 161 | 162 | A(0, 0) = init_p; 163 | A(0, 1) = 0; 164 | A(0, 2) = (12 * mid_p - 3 * final_p - 9 * init_p) / (4 * tf * tf); 165 | A(0, 3) = (-8 * mid_p + 3 * final_p + 5 * init_p) / (4 * tf * tf * tf); 166 | A(1, 0) = mid_p; 167 | A(1, 1) = (3 * final_p - 3 * init_p) / (4 * tf); 168 | A(1, 2) = (-12 * mid_p + 6 * final_p + 6 * init_p) / (4 * tf * tf); 169 | A(1, 3) = (8 * mid_p - 5 * final_p - 3 * init_p) / (4 * tf * tf * tf); 170 | 171 | return A; 172 | } 173 | 174 | VectorXd calcVel(MatrixXd theta_t, double t){ 175 | VectorXd v(6); 176 | for (int i = 0; i < 6; i++) { 177 | v(i) = theta_t(i, 1) + 2 * theta_t(i, 2) * t + 3 * theta_t(i, 3) * t * t; 178 | } 179 | return v; 180 | } -------------------------------------------------------------------------------- /robot_arm/实验4：敲铃实验（实际）/trajwithobstacle_real.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "vector" 5 | #include 6 | #include 7 | 8 | #include 9 | #include "std_msgs/Float32.h" 10 | #include "std_msgs/Float64MultiArray.h" 11 | #include "std_msgs/Float32MultiArray.h" 12 | #include "controller_manager_msgs/SwitchController.h" 13 | #include "controller_manager_msgs/ListControllers.h" 14 | #include "probot_anno_manipulator_ikfast_moveit_plugin.cpp" 15 | 16 | #define pi 3.1415926535897932384626 17 | 18 | using namespace Eigen; 19 | using namespace std; 20 | 21 | VectorXd pathInterpolation3(double init_p, double final_p, double init_v, double final_v, double period); 22 | MatrixXd pathInterp3WithMidpoint(double init_p, double mid_p, double final_p, double whole_period); 23 | VectorXd calcVel(MatrixXd theta_t, double t); 24 | 25 | int main(int argc, char** argv) 26 | { 27 | //节点初始化 28 | ros::init(argc, argv, "control_example"); 29 | //创建节点句柄对象 30 | ros::NodeHandle n; 31 | ros::AsyncSpinner spinner(1); 32 | ROS_INFO_STREAM("start"); 33 | //创建发布者对象，用于发布位置信息 34 | ros::Publisher spd_pub = n.advertise("speed_chatter", 1000); 35 | //设置信息发送频率（50Hz） 36 | ros::Rate loop_rate(50); 37 | 38 | //初始化publish的变量并初始化6个值 39 | std_msgs::Float32MultiArray spd_msg; 40 | spd_msg.data.push_back(0.0); 41 | spd_msg.data.push_back(0.0); 42 | spd_msg.data.push_back(0.0); 43 | spd_msg.data.push_back(0.0); 44 | spd_msg.data.push_back(0.0); 45 | spd_msg.data.push_back(0.0); 46 | 47 | //blue ball 0.26 0.15 0.08 48 | VectorXd blueball_joint_state(6); 49 | //blueball_joint_state << 0.523405, -1.28345, 0.0903509, -0.00186839, 1.1927, 0.525141; //real 50 | blueball_joint_state << 0.523405, -1.38345, 0.0903509, -0.00186839, 1.1927, 0.525141; //1.6s version 51 | //blueball_joint_state << 0.623405, -1.28345, 0.0903509, -0.00186839, 1.1927, 0.525141; 52 | // blueball_joint_state << 0.623405, -1.38345, 0.0903509, -0.00186839, 1.1927, 0.525141; 53 | 54 | //red ball 0.28 -0.24 0.08 55 | VectorXd redball_joint_state(6); 56 | // redball_joint_state << -0.708536, -1.44723, 0.565107, -0.000952057, 0.882642, -0.707801; 57 | //redball_joint_state << -1.208536, -1.64723, 0.565107, -0.000952057, 0.882642, -0.707801; //1.8s version 58 | redball_joint_state << -1.208536, -1.64723, 0.565107, -0.000952057, 0.882642, -0.707801;//1.6s version 59 | //redball_joint_state << -1.208536, -1.64723, 0.565107, -0.000952057, 0.882642, -0.707801; 60 | 61 | 62 | //redball_joint_state << -0.908536, -1.44723, 0.565107, -0.000952057, 0.882642, -0.707801; 63 | // redball_joint_state << -0.908536, -1.64723, 0.565107, -0.000952057, 0.882642, -0.707801; 64 | 65 | 66 | /******************************adjust the three parameters to control speed ****************************/ 67 | //middle point, which is supposed to be calculated by inverse kinematics, while replaced with empirical numbers 68 | VectorXd midpoint_joint_state(6); 69 | //midpoint_joint_state << 0, -1.2, 0.45, 0, 0.8, 0; // further from the obstacle,meng's version 70 | midpoint_joint_state << -0.131790011092997, -1.212079770113469, 0.477135176977792, 0.009279848335710, 0.784490189836066, -0.138183182148361;//1.6s version 71 | //midpoint_joint_state << -0.131790011092997, -0.912079770113469, 0.477135176977792, 0.009279848335710, 0.784490189836066, -0.138183182148361;//1.5s version 72 | //midpoint_joint_state << 0, -1.4, 0.6, 0, 0.8, 0; // closer to the obstacle 73 | //midpoint_joint_state << 0.000115258, -1.15935, 0.450616, -0.00104892, 0.708735, 0.000797974; 74 | 75 | double timeperiod_s2b = 5; // the time spent from start point to reaching the blue ball for the first time 76 | double timeperiod_r2b = 1.46; // the time spent from red ball to blue ball (same time spent on blue to red) 77 | /******************************adjust the three parameters to control speed****************************/ 78 | 79 | MatrixXd start2blue_joint_state_interp_param(6,4); // the coeffients of the joint state interpolation polynomial from start to blue ball 80 | MatrixXd blue2mid_joint_state_interp_param(6,4); 81 | MatrixXd mid2red_joint_state_interp_param(6,4); 82 | MatrixXd red2mid_joint_state_interp_param(6,4); 83 | MatrixXd mid2blue_joint_state_interp_param(6,4); 84 | 85 | // calculate the coefficients of the interpolation polynomials 86 | for (int i = 0; i < 6; i++){ 87 | start2blue_joint_state_interp_param.block <1, 4>(i, 0) = pathInterpolation3(0, blueball_joint_state(i), 0, 0, timeperiod_s2b).transpose(); 88 | blue2mid_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(blueball_joint_state(i), midpoint_joint_state(i), redball_joint_state(i), timeperiod_r2b).block <1,4>(0,0); 89 | mid2red_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(blueball_joint_state(i), midpoint_joint_state(i), redball_joint_state(i), timeperiod_r2b).block <1,4>(1,0); 90 | red2mid_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(redball_joint_state(i), midpoint_joint_state(i), blueball_joint_state(i), timeperiod_r2b).block <1,4>(0,0); 91 | mid2blue_joint_state_interp_param.block <1, 4>(i, 0) = pathInterp3WithMidpoint(redball_joint_state(i), midpoint_joint_state(i), blueball_joint_state(i), timeperiod_r2b).block <1,4>(1,0); 92 | } 93 | 94 | //from start point to reaching blue ball for the first time 95 | double t = 0.0; 96 | const double timestep = 0.02; 97 | VectorXd vel(6); 98 | while (ros::ok()) 99 | { 100 | if (t >= timeperiod_s2b) break; 101 | 102 | vel = calcVel(start2blue_joint_state_interp_param, t); 103 | 104 | spd_msg.data.at(0) = vel(0) * 30 * 180 / pi; 105 | spd_msg.data.at(1) = vel(1) * 205 * 180 / (3 * pi); 106 | spd_msg.data.at(2) = vel(2) * 50 * 180 / pi; 107 | spd_msg.data.at(3) = vel(3) * 125 * 180 / (2 * pi); 108 | spd_msg.data.at(4) = vel(4) * 125 * 180 / (2 * pi); 109 | spd_msg.data.at(5) = vel(5) * 200 * 180 / (9 * pi); 110 | spd_pub.publish(spd_msg); 111 | ROS_INFO_STREAM("published"); 112 | 113 | t = t + timestep; 114 | loop_rate.sleep(); 115 | } 116 | double max_vel = 0; 117 | //back and forth between blue and red ball 118 | int current_state = 0; // 0: blue2mid, 1: mid2red, 2: red2mid, 3: mid2blue 119 | while (ros::ok()){ 120 | for(t = 0; t <= timeperiod_r2b / 2; t += timestep, loop_rate.sleep()){ 121 | switch(current_state){ 122 | case 0: 123 | vel = calcVel (blue2mid_joint_state_interp_param, t); 124 | break; 125 | case 1: 126 | vel = calcVel (mid2red_joint_state_interp_param, t); 127 | break; 128 | case 2: 129 | vel = calcVel (red2mid_joint_state_interp_param, t); 130 | break; 131 | case 3: 132 | vel = calcVel (mid2blue_joint_state_interp_param, t); 133 | break; 134 | } 135 | 136 | spd_msg.data.at(0) = vel(0) * 30 * 180 / pi; 137 | spd_msg.data.at(1) = vel(1) * 205 * 180 / (3 * pi); 138 | spd_msg.data.at(2) = vel(2) * 50 * 180 / pi; 139 | spd_msg.data.at(3) = vel(3) * 125 * 180 / (2 * pi); 140 | spd_msg.data.at(4) = vel(4) * 125 * 180 / (2 * pi); 141 | spd_msg.data.at(5) = vel(5) * 200 * 180 / (9 * pi); 142 | spd_pub.publish(spd_msg); 143 | ROS_INFO_STREAM("published"); 144 | } 145 | current_state += 1; 146 | if(current_state == 4) current_state = 0; 147 | } 148 | return 0; 149 | } 150 | 151 | 152 | //Interpolation: p = a0 + a1 * t + a2 * t^2 + a3 * t^3 153 | //arg: init_p, final_p, init_v, final_v, period 154 | //return: [a0, a1, a2, a3] 155 | VectorXd pathInterpolation3(double init_p, double final_p, double init_v, double final_v, double period){ 156 | VectorXd param(4); 157 | 158 | param(0) = init_p; 159 | param(1) = init_v; 160 | param(2) = 3 * (final_p - init_p) / pow(period, 2) - 2 * init_v / period - final_v / period; 161 | param(3) = -2 * (final_p - init_p) / pow(period, 3) + (final_v + init_v) / pow(period, 2); 162 | 163 | return param; 164 | } 165 | 166 | //Interpolation with midpoint: p = a0 + a1 * t + a2 * t^2 + a3 * t^3 167 | //arg: init_p, final_p, init_v, final_v, whole_period 168 | //return: [a10, a11, a12, a13 169 | // a20, a21, a22, a23] 170 | MatrixXd pathInterp3WithMidpoint(double init_p, double mid_p, double final_p, double whole_period){ 171 | //tf1 = tf2 172 | double tf = whole_period / 2; 173 | MatrixXd A(2, 4); 174 | 175 | A(0, 0) = init_p; 176 | A(0, 1) = 0; 177 | A(0, 2) = (12 * mid_p - 3 * final_p - 9 * init_p) / (4 * tf * tf); 178 | A(0, 3) = (-8 * mid_p + 3 * final_p + 5 * init_p) / (4 * tf * tf * tf); 179 | A(1, 0) = mid_p; 180 | A(1, 1) = (3 * final_p - 3 * init_p) / (4 * tf); 181 | A(1, 2) = (-12 * mid_p + 6 * final_p + 6 * init_p) / (4 * tf * tf); 182 | A(1, 3) = (8 * mid_p - 5 * final_p - 3 * init_p) / (4 * tf * tf * tf); 183 | 184 | return A; 185 | } 186 | 187 | VectorXd calcVel(MatrixXd theta_t, double t){ 188 | VectorXd v(6); 189 | for (int i = 0; i < 6; i++) { 190 | v(i) = theta_t(i, 1) + 2 * theta_t(i, 2) * t + 3 * theta_t(i, 3) * t * t; 191 | } 192 | return v; 193 | } -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/course_agv_nav/.DS_Store -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(course_agv_nav) 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 | rospy 12 | std_msgs 13 | message_generation 14 | ) 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | add_service_files( 57 | FILES 58 | Plan.srv 59 | # Service1.srv 60 | # Service2.srv 61 | ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | generate_messages( 72 | DEPENDENCIES 73 | std_msgs # Or other packages containing msgs 74 | ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES course_agv_nav 108 | CATKIN_DEPENDS message_runtime 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | # ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/course_agv_nav.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/course_agv_nav_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_course_agv_nav.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/launch/nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/launch/replan.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | course_agv_nav 4 | 0.0.0 5 | The course_agv_nav package 6 | 7 | 8 | 9 | 10 | zjunlict 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 | message_generation 43 | 44 | 45 | 46 | message_runtime 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/scripts/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/course_agv_nav/scripts/.DS_Store -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/scripts/dwa.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import rospy 4 | import tf 5 | 6 | """ 7 | Dynamic window approach implementation with python 8 | Reference: The Dynamic Window Approach to Collision Avoidance 9 | https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf 10 | """ 11 | class DWAPlanner(): 12 | def __init__(self): 13 | # robot parameter 14 | self.min_v_speed = 0 15 | self.max_v_speed = 0.8 # [m/s] 16 | self.min_w_speed = -0.5 17 | self.max_w_speed = 0.5 18 | self.predict_time = 2 # [s] 19 | self.va = 0.2 20 | self.wa = 0.1 21 | self.v_step = 0.2 22 | self.w_step = 0.1 23 | self.radius = 0.15 24 | self.delta_t = 0.4 25 | self.alpha = 4 #goal 26 | self.beta = 1 #velocity 27 | self.gamma = 2 #ob 28 | self.avoid_size = 0.05 29 | 30 | def plan(self, x, goal, ob): 31 | u = x[3:5] 32 | u = self.dwa_search(x,u,goal,ob) 33 | return u 34 | 35 | def dwa_search(self, X, u, goal, obstacles): 36 | vw=self.calculate_vw_range(X) 37 | min_score = 10000000000 38 | i = 0 39 | print('vw[0],vw[1]:', vw[0], vw[1]) 40 | print('vw[2],vw[3]:', vw[2], vw[3]) 41 | 42 | for v in np.arange(vw[0], vw[1], self.v_step): 43 | for w in np.arange(vw[2], vw[3], self.w_step): 44 | traj = self.predict_trajectory(X,[v,w]) 45 | # if self.check_collision(traj,obstacles) == True: 46 | # print('check_collision == True') 47 | # continue 48 | goal_score = self.heading_obj_func(traj, goal) 49 | vel_score = self.velocity_obj_func(traj) 50 | obs_score = self.clearance_obj_func(traj,obstacles) 51 | score = self.alpha * goal_score + self.beta * vel_score + self.gamma * obs_score 52 | if score <= min_score: 53 | min_score = score 54 | u = np.array([v,w]) 55 | i += 1 56 | return u 57 | 58 | def calculate_vw_range(self, X): 59 | v_min = X[3] - self.va * self.predict_time 60 | v_max = X[3] + self.va * self.predict_time 61 | w_min = X[4] - self.wa * self.predict_time 62 | w_max = X[4] + self.wa * self.predict_time 63 | 64 | VW = [max(self.min_v_speed,v_min),min(self.max_v_speed,v_max),max(self.min_w_speed,w_min),min(self.max_w_speed,w_max)] 65 | return VW 66 | 67 | def Motion(self,X,u): 68 | X[0]+=u[0] * self.delta_t * math.cos(X[2]) 69 | X[1]+=u[0] * self.delta_t * math.sin(X[2]) 70 | X[2]+=u[1] * self.delta_t 71 | X[3]=u[0] 72 | X[4]=u[1] 73 | return X 74 | 75 | def predict_trajectory(self, x_init, u): 76 | """ 77 | Predict trajectory: return trajectory in predict time 78 | """ 79 | Traj = np.array(x_init) 80 | Xnew = np.array(x_init) 81 | 82 | time=0 83 | while time <= self.predict_time: 84 | Xnew=self.Motion(Xnew,u) 85 | 86 | time = time + self.delta_t 87 | Traj = np.vstack((Traj,Xnew)) 88 | return Traj 89 | 90 | pass 91 | 92 | def check_collision(self, trajectory, ob): 93 | """ 94 | Check Collision: return true if collision happens 95 | """ 96 | 97 | for i in range(len(trajectory)): 98 | for j in range(len(ob)): 99 | dist = math.sqrt((trajectory[i,0] - ob[j,0]) ** 2 + (trajectory[i,1] - ob[j,1]) ** 2) 100 | 101 | if dist <= self.radius + self.avoid_size: 102 | print('dist:', dist) 103 | return True 104 | 105 | return False 106 | 107 | pass 108 | 109 | def heading_obj_func(self, trajectory, goal): 110 | """ 111 | Target heading: heading is min_v_speed measure of progress towards the goal location. 112 | It is maximal if the robot moves directly towards the target. 113 | """ 114 | return math.sqrt((trajectory[-1,0] - goal[0]) ** 2 + (trajectory[-1,1] - goal[1]) ** 2) 115 | pass 116 | 117 | # angle1 = math.atan2(goal[1]-trajectory[-1,1], goal[0]-trajectory[-1,0]) 118 | # angle2 = trajectory[-1,2] 119 | # delta = abs(angle1-angle2) 120 | # if abs(angle1-angle2)>math.pi: 121 | # delta = 2*math.pi - delta 122 | 123 | # return delta 124 | 125 | def clearance_obj_func(self, trajectory, ob): 126 | """ 127 | Clearance: dist is the distance to the closest obstacle on the trajectory. 128 | The smaller the distance to an obstacle the higher is the robot's desire to move around it. 129 | """ 130 | min_dist = 1000000000000 131 | # dist_array = np.array([]) 132 | for i in range(len(trajectory)): 133 | for j in range(len(ob)): 134 | dist = math.sqrt((trajectory[i,0] - ob[j,0]) ** 2 + (trajectory[i,1] - ob[j,1]) ** 2) 135 | if dist < self.radius: 136 | return float('Inf') 137 | 138 | # dist_array = np.append(dist_array, dist) 139 | 140 | if dist < min_dist: 141 | min_dist = dist 142 | flag = j 143 | 144 | return 1/min_dist 145 | pass 146 | 147 | def velocity_obj_func(self, trajectory): 148 | """ 149 | Velocity: vel is the forward velocity of the robot and supports fast movements. 150 | """ 151 | return self.max_v_speed - trajectory[-1,3] 152 | pass 153 | -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/scripts/global_planner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | from nav_msgs.srv import GetMap 5 | from nav_msgs.msg import Path 6 | from geometry_msgs.msg import PoseStamped 7 | from course_agv_nav.srv import Plan,PlanResponse 8 | from nav_msgs.msg import OccupancyGrid 9 | from std_msgs.msg import Bool 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | 13 | m rrt import RRTPlanner 14 | 15 | class GlobalPlanner: 16 | def __init__(self): 17 | self.plan_sx = 0.0 18 | self.plan_sy = 0.0 19 | self.plan_gx = 8.0 20 | self.plan_gy = -8.0 21 | self.plan_grid_size = 0.3 22 | self.plan_robot_radius = 0.3 23 | self.plan_ox = [] 24 | self.plan_oy = [] 25 | self.plan_rx = [] 26 | self.plan_ry = [] 27 | 28 | # count to update map 29 | self.map_count = 0 30 | 31 | self.tf = tf.TransformListener() 32 | self.goal_sub = rospy.Subscriber('/course_agv/goal',PoseStamped,self.goalCallback) 33 | self.plan_srv = rospy.Service('/course_agv/global_plan',Plan,self.replan) 34 | self.path_pub = rospy.Publisher('course_agv/global_path',Path,queue_size = 1) 35 | self.map_sub = rospy.Subscriber('/map',OccupancyGrid,self.mapCallback) 36 | self.collision_sub = rospy.Subscriber('/collision_checker_result',Bool,self.collisionCallback) 37 | 38 | # self.updateMap() 39 | self.updateGlobalPose() 40 | 41 | pass 42 | def goalCallback(self,msg): 43 | self.plan_goal = msg 44 | self.plan_gx = msg.pose.position.x 45 | self.plan_gy = msg.pose.position.y 46 | print("get new goal!!! ",self.plan_goal) 47 | self.replan(0) 48 | pass 49 | 50 | def collisionCallback(self,msg): 51 | self.replan(0) 52 | def updateGlobalPose(self): 53 | try: 54 | self.tf.waitForTransform("/map", "/base_footprint", rospy.Time(), rospy.Duration(4.0)) 55 | (self.trans,self.rot) = self.tf.lookupTransform('/map','/base_footprint',rospy.Time(0)) 56 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 57 | print("get tf error!") 58 | self.plan_sx = self.trans[0] 59 | self.plan_sy = self.trans[1] 60 | print("debug start ",self.plan_sx,self.plan_sy) 61 | 62 | def replan(self,req): 63 | print('get request for replan!!!!!!!!') 64 | self.initAStar() 65 | self.updateGlobalPose() 66 | print('a') 67 | self.plan_rx,self.plan_ry = self.a_star.plan(self.plan_sx,self.plan_sy,self.plan_gx,self.plan_gy) 68 | print('b') 69 | 70 | print(self.plan_rx) 71 | print(self.plan_ry) 72 | self.publishPath() 73 | res = True 74 | return PlanResponse(res) 75 | 76 | def initAStar(self): 77 | print("debugmap",self.map.info.width,self.map.info.height) 78 | map_data = np.array(self.map.data).reshape((self.map.info.height,-1)).transpose() 79 | ox,oy = np.nonzero(map_data < 0) 80 | self.plan_ox = (ox*self.map.info.resolution+self.map.info.origin.position.x).tolist() 81 | self.plan_oy = (oy*self.map.info.resolution+self.map.info.origin.position.y).tolist() 82 | # self.a_star = AStarPlanner(ox=self.plan_ox, oy=self.plan_oy, step_size=self.plan_grid_size,robot_radius=self.plan_robot_radius) 83 | self.a_star = RRTPlanner(self.plan_ox, self.plan_oy, 0.15, 0.25,-10,-10,10,10) 84 | 85 | 86 | 87 | def mapCallback(self,msg): 88 | print('Get Map!') 89 | self.map = msg 90 | pass 91 | def updateMap(self): 92 | rospy.wait_for_service('/map') 93 | try: 94 | getMap = rospy.ServiceProxy('/map',GetMap) 95 | msg = getMap().map 96 | except: 97 | e = sys.exc_info()[0] 98 | print('Service call failed: %s'%e) 99 | # Update for planning algorithm 100 | self.mapCallback(msg) 101 | 102 | def publishPath(self): 103 | print("markdebug ",self.plan_rx,self.plan_ry) 104 | path = Path() 105 | path.header.seq = 0 106 | path.header.stamp = rospy.Time(0) 107 | path.header.frame_id = 'map' 108 | for i in range(len(self.plan_rx)): 109 | pose = PoseStamped() 110 | pose.header.seq = i 111 | pose.header.stamp = rospy.Time(0) 112 | pose.header.frame_id = 'map' 113 | # pose.pose.position.x = self.plan_rx[len(self.plan_rx)-1-i] 114 | # pose.pose.position.y = self.plan_ry[len(self.plan_rx)-1-i] 115 | pose.pose.position.x = self.plan_rx[i] 116 | pose.pose.position.y = self.plan_ry[i] 117 | pose.pose.position.z = 0.01 118 | pose.pose.orientation.x = 0 119 | pose.pose.orientation.y = 0 120 | pose.pose.orientation.z = 0 121 | pose.pose.orientation.w = 1 122 | path.poses.append(pose) 123 | self.path_pub.publish(path) 124 | print('publish path!') 125 | 126 | 127 | def main(): 128 | rospy.init_node('global_planner') 129 | gp = GlobalPlanner() 130 | rospy.spin() 131 | pass 132 | 133 | if __name__ == '__main__': 134 | 135 | main() 136 | -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/scripts/local_planner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | import numpy as np 4 | import rospy 5 | import tf 6 | from nav_msgs.msg import Path 7 | from geometry_msgs.msg import PoseStamped,Twist 8 | from sensor_msgs.msg import LaserScan 9 | import math 10 | from dwa import DWAPlanner 11 | 12 | from threading import Lock,Thread 13 | import time 14 | 15 | def limitVal(minV,maxV,v): 16 | if v < minV: 17 | return minV 18 | if v > maxV: 19 | return maxV 20 | return v 21 | 22 | class LocalPlanner: 23 | def __init__(self): 24 | self.arrive = 0.1 25 | self.x = 0.0 26 | self.y = 0.0 27 | self.yaw = 0.0 28 | self.vx = 0.0 29 | self.vw = 0.0 30 | # init plan_config for once 31 | self.dwa = DWAPlanner() 32 | self.threshold = self.dwa.max_v_speed*self.dwa.predict_time 33 | self.thres = 0.3 34 | 35 | self.laser_lock = Lock() 36 | self.lock = Lock() 37 | self.path = Path() 38 | 39 | self.tf = tf.TransformListener() 40 | self.path_sub = rospy.Subscriber('/course_agv/global_path',Path,self.pathCallback) 41 | self.vel_pub = rospy.Publisher('/webService/cmd_vel',Twist, queue_size=1) 42 | self.midpose_pub = rospy.Publisher('/course_agv/mid_goal',PoseStamped,queue_size=1) 43 | self.laser_sub = rospy.Subscriber('/scan_emma_nav_front',LaserScan,self.laserCallback) 44 | self.planner_thread = None 45 | 46 | self.count = 0 47 | pass 48 | def updateGlobalPose(self): 49 | try: 50 | self.tf.waitForTransform("/map", "/base_footprint", rospy.Time(), rospy.Duration(4.0)) 51 | (self.trans,self.rot) = self.tf.lookupTransform('/map','/base_footprint',rospy.Time(0)) 52 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 53 | print("get tf error!") 54 | euler = tf.transformations.euler_from_quaternion(self.rot) 55 | roll,pitch,yaw = euler[0],euler[1],euler[2] 56 | self.x = self.trans[0] 57 | self.y = self.trans[1] 58 | self.yaw = yaw 59 | 60 | def laserCallback(self,msg): 61 | self.laser_lock.acquire() 62 | # preprocess 63 | self.ob = [[100,100]] 64 | angle_min = msg.angle_min 65 | angle_increment = msg.angle_increment 66 | for i in range(len(msg.ranges)): 67 | a = angle_min + angle_increment*i 68 | r = msg.ranges[i] 69 | if r < 1.6 and r > 0.1: 70 | self.ob.append([math.cos(a)*r,math.sin(a)*r]) 71 | self.laser_lock.release() 72 | pass 73 | 74 | def u2t(self, u): 75 | w = u[2] 76 | x = u[0] 77 | y = u[1] 78 | return np.array([[math.cos(w),-math.sin(w),x],[math.sin(w),math.cos(w),y],[0,0,1]]) 79 | 80 | def updateObstacle(self): 81 | self.laser_lock.acquire() 82 | self.plan_ob = [] 83 | self.plan_ob = np.array(self.ob) 84 | 85 | #do transformation from robot coordinate to global coordinate 86 | trans = self.u2t([self.x, self.y, self.yaw]) 87 | temp = np.ones([len(self.plan_ob),1]) 88 | self.plan_ob = np.concatenate((self.plan_ob,temp),axis=1) 89 | self.plan_ob = trans.dot(self.plan_ob.T) 90 | self.plan_ob = self.plan_ob.T 91 | self.plan_ob = self.plan_ob[:,0:2] 92 | 93 | self.laser_lock.release() 94 | pass 95 | def pathCallback(self,msg): 96 | self.path = msg 97 | self.lock.acquire() 98 | self.initPlanning() 99 | self.lock.release() 100 | # if self.planner_thread == None: 101 | # self.planner_thread = Thread(target=self.planThreadFunc) 102 | # self.planner_thread.start() 103 | # pass 104 | self.planThreadFunc() 105 | def initPlanning(self): 106 | self.goal_index = 0 107 | self.vx = 0.0 108 | self.vw = 0.0 109 | self.dis = 99999 110 | self.updateGlobalPose() 111 | cx = [] 112 | cy = [] 113 | for pose in self.path.poses: 114 | cx.append(pose.pose.position.x) 115 | cy.append(pose.pose.position.y) 116 | self.goal = np.array([cx[0],cy[0]]) 117 | self.plan_cx,self.plan_cy = np.array(cx),np.array(cy) 118 | self.destination = np.array([cx[-1],cy[-1]]) 119 | self.plan_goal = np.array([cx[-1],cy[-1]]) 120 | self.plan_x = np.array([0.0,0.0,0.0,self.vx,self.vw]) 121 | pass 122 | 123 | def planThreadFunc(self): 124 | print("running planning thread!!") 125 | i = 0 126 | for pose in self.path.poses: 127 | if i == 0: 128 | i = 1 129 | continue 130 | else: 131 | self.goal_node = np.array([pose.pose.position.x, pose.pose.position.y]) 132 | 133 | goal = pose 134 | self.midpose_pub.publish(goal) 135 | self.goal_node = np.array([goal.pose.position.x,goal.pose.position.y]) 136 | while True: 137 | self.lock.acquire() 138 | # start = time.time() 139 | # self.planOnce() 140 | end = time.time() 141 | # print('this plan time cost:', end-start) 142 | self.lock.release() 143 | self.updateGlobalPose() 144 | self.goal_dis = math.hypot(self.x-pose.pose.position.x, self.y-pose.pose.position.y) 145 | print('goal_dis:', self.goal_dis) 146 | if i == len(self.path.poses) - 1: 147 | threshold = 0.3 148 | else: 149 | threshold = 0.4 150 | if self.goal_dis < 0.4: 151 | print("arrive one goal node!") 152 | time.sleep(0.001) 153 | self.lock.acquire() 154 | self.publishVel(True) 155 | self.lock.release() 156 | self.planning_thread = None 157 | i += 1 158 | 159 | self.publishVel(True) 160 | print('at goal！') 161 | 162 | 163 | 164 | 165 | 166 | 167 | def planOnce(self): 168 | self.updateGlobalPose() 169 | # Update plan_x [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] 170 | self.plan_x = [self.x,self.y,self.yaw,self.vx,self.vw] 171 | # Update obstacle 172 | self.updateObstacle() 173 | u = self.dwa.plan(self.plan_x, self.goal_node, self.plan_ob) 174 | alpha = 0.5 175 | self.vx = u[0]*alpha + self.vx*(1-alpha) 176 | self.vw = u[1]*alpha + self.vw*(1-alpha) 177 | print("v, w: ", self.vx, self.vw) 178 | # print("mdbg; ",u) 179 | self.publishVel() 180 | pass 181 | 182 | def publishVel(self,zero = False): 183 | if zero: 184 | self.vx = 0 185 | self.vw = 0 186 | cmd = Twist() 187 | cmd.linear.x = self.vx 188 | cmd.angular.z = self.vw 189 | self.vel_pub.publish(cmd) 190 | 191 | def main(): 192 | rospy.init_node('path_Planning') 193 | lp = LocalPlanner() 194 | rospy.spin() 195 | 196 | if __name__ == '__main__': 197 | main() 198 | -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/scripts/rrt.py: -------------------------------------------------------------------------------- 1 | from scipy.spatial import KDTree 2 | import numpy as np 3 | import random 4 | import math 5 | import time 6 | 7 | """ 8 | RRT path planning implementation with python 9 | """ 10 | class RRTPlanner(): 11 | def __init__(self, ox, oy, robot_radius, avoid_buffer,minx,miny,maxx,maxy): 12 | self.obstacle_x = ox 13 | self.obstacle_y = oy 14 | 15 | self.minx = minx 16 | self.maxx = maxx 17 | self.miny = miny 18 | self.maxy = maxy 19 | self.robot_size = robot_radius 20 | self.avoid_dist = avoid_buffer 21 | self.rang=1 22 | self.Nr=1000 23 | self.delta=1.5 24 | self.MAX_EDGE_LEN = maxx-minx 25 | pass 26 | 27 | def plan(self, start_x, start_y, goal_x, goal_y): 28 | # Obstacles 29 | 30 | # Obstacle KD Tree 31 | # print(np.vstack((obstacle_x, obstacle_y)).T) 32 | obstree = KDTree(np.vstack((self.obstacle_x, self.obstacle_y)).T) 33 | # Building tree 34 | print('c') 35 | pt_x, pt_y,pt_f,endnum = self.building(start_x, start_y, goal_x, goal_y, obstree) 36 | print('d') 37 | 38 | # Search Path 39 | path_x, path_y = self.backpath(pt_x,pt_y,pt_f,endnum) 40 | path_x,path_y=self.shrink(path_x,path_y,obstree) 41 | path_x.reverse() 42 | path_y.reverse() 43 | 44 | return path_x, path_y 45 | 46 | def building(self, start_x, start_y, goal_x, goal_y, obstree): 47 | pt_x, pt_y,pt_f = [], [],[] 48 | pt_x.append(start_x) 49 | pt_y.append(start_y) 50 | pt_f.append(-1) 51 | for count in range(self.Nr): 52 | tx = (random.random() * (self.maxx - self.minx)) + self.minx 53 | ty = (random.random() * (self.maxy - self.miny)) + self.miny 54 | 55 | #distance, index = obstree.query(np.array([tx, ty])) 56 | 57 | #if distance >= self.robot_size + self.avoid_dist: 58 | # continue 59 | ptstree=KDTree(np.vstack((pt_x, pt_y)).T) 60 | distance, index = ptstree.query(np.array([tx, ty])) 61 | qnearst_x=pt_x[index] 62 | qnearst_y=pt_y[index] 63 | if math.hypot(qnearst_x-goal_x,qnearst_y-goal_y)=0: 88 | path_x.append(pt_x[endnum]) 89 | path_y.append(pt_y[endnum]) 90 | endnum=pt_f[endnum] 91 | 92 | return path_x,path_y 93 | 94 | def shrink(self,path_x,path_y,obstree): 95 | length=len(path_x) 96 | i=length-1 97 | while i > 1: 98 | if not self.check_obs(path_x[i],path_y[i],path_x[i-2],path_y[i-2],obstree): 99 | path_x.remove(path_x[i-1]) 100 | path_y.remove(path_y[i-1]) 101 | 102 | i=i-1 103 | 104 | return path_x,path_y 105 | 106 | def check_obs(self, ix, iy, nx, ny, obstree): 107 | x = ix 108 | y = iy 109 | dx = nx - ix 110 | dy = ny - iy 111 | angle = math.atan2(dy, dx) 112 | dis = math.hypot(dx, dy) 113 | 114 | if dis > self.MAX_EDGE_LEN: 115 | return True 116 | 117 | step_size = self.robot_size + self.avoid_dist 118 | steps = int(round(dis/step_size)) 119 | # print(dis) 120 | # print(step_size) 121 | # print(steps) 122 | for i in range(steps): 123 | distance, index = obstree.query(np.array([x, y])) 124 | if distance <= self.robot_size + self.avoid_dist: 125 | return True 126 | x += step_size * math.cos(angle) 127 | y += step_size * math.sin(angle) 128 | 129 | # check for goal point 130 | distance, index = obstree.query(np.array([nx, ny])) 131 | if distance <= self.robot_size + self.avoid_dist: 132 | return True 133 | 134 | return False 135 | -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/scripts/stupid_tracking.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | #!/usr/bin/env python 4 | import rospy 5 | import tf 6 | from nav_msgs.msg import Path 7 | from geometry_msgs.msg import PoseStamped,Twist 8 | from sensor_msgs.msg import LaserScan 9 | from threading import Lock,Thread 10 | import math 11 | import time 12 | class Tracking: 13 | def __init__(self): 14 | self.arrive = 0.1 15 | self.arrive_threshold = 0.3 16 | self.vx = 0.0 17 | self.vw = 0.0 18 | 19 | self.lock = Lock() 20 | self.path = Path() 21 | self.tf = tf.TransformListener() 22 | # self.path_sub = rospy.Subscriber('/course_agv/global_path',Path,self.pathCallback) 23 | # self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback) 24 | # self.vel_pub = rospy.Publisher('/course_agv/velocity',Twist, queue_size=1) 25 | # self.midpose_pub = rospy.Publisher('/course_agv/mid_goal',PoseStamped,queue_size=1) 26 | self.path_sub = rospy.Subscriber('/course_agv/global_path',Path,self.pathCallback) 27 | self.vel_pub = rospy.Publisher('/webService/cmd_vel',Twist, queue_size=1) 28 | self.midpose_pub = rospy.Publisher('/course_agv/mid_goal',PoseStamped,queue_size=1) 29 | self.laser_sub = rospy.Subscriber('/scan',LaserScan,self.laserCallback) 30 | self.tracking_thread = None 31 | pass 32 | def laserCallback(self,msg): 33 | self.received = True 34 | def updateGlobalPose(self): 35 | try: 36 | self.tf.waitForTransform("/map", "/base_footprint", rospy.Time(), rospy.Duration(4.0)) 37 | (self.trans,self.rot) = self.tf.lookupTransform('/map','/base_footprint',rospy.Time(0)) 38 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 39 | print("get tf error!") 40 | euler = tf.transformations.euler_from_quaternion(self.rot) 41 | roll,pitch,yaw = euler[0],euler[1],euler[2] 42 | self.x = self.trans[0] 43 | self.y = self.trans[1] 44 | self.yaw = yaw 45 | 46 | p = self.path.poses[self.goal_index].pose.position 47 | dis = math.hypot(p.x-self.x,p.y-self.y) 48 | if dis < self.arrive_threshold and self.goal_index < len(self.path.poses)-1: 49 | self.goal_index = self.goal_index + 1 50 | self.midpose_pub.publish(self.path.poses[self.goal_index]) 51 | self.goal_dis = math.hypot(self.x-self.path.poses[-1].pose.position.x,self.y-self.path.poses[-1].pose.position.y) 52 | 53 | def pathCallback(self,msg): 54 | # print("get path msg!!!!!",msg) 55 | self.path = msg 56 | self.lock.acquire() 57 | self.initTracking() 58 | self.lock.release() 59 | if self.tracking_thread == None: 60 | self.tracking_thread = Thread(target=self.trackThreadFunc) 61 | self.tracking_thread.start() 62 | pass 63 | def initTracking(self): 64 | self.goal_index = 0 65 | self.updateGlobalPose() 66 | pass 67 | def trackThreadFunc(self): 68 | print("running track thread!!") 69 | # while self.plan_lastIndex > self.plan_target_ind: 70 | while True: 71 | time.sleep(0.1) 72 | if self.received == False: 73 | continue 74 | self.received = False 75 | self.planOnce() 76 | if self.goal_dis < self.arrive: 77 | print("arrive goal!") 78 | break 79 | print("exit track thread!!") 80 | self.lock.acquire() 81 | self.publishVel(True) 82 | # time.sleep(0.001) 83 | self.lock.release() 84 | self.tracking_thread = None 85 | pass 86 | def planOnce(self): 87 | self.lock.acquire() 88 | 89 | self.updateGlobalPose() 90 | 91 | target = self.path.poses[self.goal_index].pose.position 92 | 93 | dx = target.x - self.x 94 | dy = target.y - self.y 95 | 96 | target_angle = math.atan2(dy, dx) 97 | 98 | per_step = 0.001 99 | self.vw = self.pi2pi(target_angle-self.yaw)/2.0 100 | self.vw = self.clip(self.vw,-0.3,0.3) 101 | 102 | if abs(self.vw) > 0.1: 103 | self.vx = self.clip(0,self.vx-per_step,self.vx+per_step) 104 | else: 105 | self.vx = self.clip(math.hypot(dx,dy)/2.0,self.vx-per_step,self.vx+per_step) 106 | self.vx = self.clip(self.vx,0,0.5) 107 | self.publishVel() 108 | 109 | self.lock.release() 110 | pass 111 | def clip(self,v,minv,maxv): 112 | if v < minv: 113 | return minv 114 | if v > maxv: 115 | return maxv 116 | return v 117 | 118 | def pi2pi(self,angle): 119 | a = angle%(2*math.pi) 120 | if a > math.pi: 121 | a = a - 2*math.pi 122 | return a 123 | 124 | def publishVel(self,zero = False): 125 | cmd = Twist() 126 | print("v, w", self.vx, self.vw) 127 | # self.vx, self.vw = 0, 0 128 | cmd.linear.x = self.vx 129 | cmd.angular.z = self.vw 130 | if zero: 131 | cmd.linear.x = 0 132 | cmd.angular.z = 0 133 | self.vel_pub.publish(cmd) 134 | 135 | def main(): 136 | rospy.init_node('stupid_tracking') 137 | t = Tracking() 138 | rospy.spin() 139 | 140 | def test(t): 141 | rx = [0,1,2,3,4,5,6,7] 142 | ry = [1,0,1,0,1,0,1,0] 143 | path = Path() 144 | path.header.seq = 0 145 | path.header.stamp = rospy.Time(0) 146 | path.header.frame_id = 'map' 147 | for i in range(len(rx)): 148 | pose = PoseStamped() 149 | pose.header.seq = i 150 | pose.header.stamp = rospy.Time(0) 151 | pose.header.frame_id = 'map' 152 | pose.pose.position.x = rx[i] 153 | pose.pose.position.y = ry[i] 154 | pose.pose.position.z = 0.01 155 | pose.pose.orientation.x = 0#self.rot[0] 156 | pose.pose.orientation.y = 0#self.rot[1] 157 | pose.pose.orientation.z = 0#self.rot[2] 158 | pose.pose.orientation.w = 1#self.rot[3] 159 | path.poses.append(pose) 160 | pub = rospy.Publisher('/course_agv/global_path',Path,queue_size = 10) 161 | time.sleep(0.5) 162 | pub.publish(path) 163 | print("publish path!!!!!") 164 | # t.pathCallback(path) 165 | 166 | if __name__ == '__main__': 167 | main() -------------------------------------------------------------------------------- /stupid_yellow_car/course_agv_nav/srv/Plan.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool res -------------------------------------------------------------------------------- /stupid_yellow_car/images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/1.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-11 15-01-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-11 15-01-05屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-12 23-21-35屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-12 23-21-35屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-15 11-41-24屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-15 11-41-24屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-17 11-13-15屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-17 11-13-15屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-18 22-34-54屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-18 22-34-54屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-19 10-57-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-19 10-57-05屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-19 14-58-22屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-19 14-58-22屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-19 14-58-48屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-19 14-58-48屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-19 14-59-16屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-19 14-59-16屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-19 15-04-03屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-19 15-04-03屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-19 15-09-58屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-19 15-09-58屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-19 15-12-31屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-19 15-12-31屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-19 15-12-47屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-19 15-12-47屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-19 15-15-48屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-19 15-15-48屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-23-13屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-23-13屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-25-01屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-25-01屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-30-08屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-30-08屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-30-19屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-30-19屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-32-22屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-32-22屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-32-37屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-32-37屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-35-12屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-35-12屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-36-23屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-36-23屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-37-23屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-37-23屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 10-40-06屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 10-40-06屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 13-57-03屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 13-57-03屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 13-57-22屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 13-57-22屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 14-01-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 14-01-05屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-21 14-04-46屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-21 14-04-46屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-23 21-23-14屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-23 21-23-14屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-23 22-05-52屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-23 22-05-52屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-24 20-45-05屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-24 20-45-05屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-24 21-05-44屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-24 21-05-44屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-24 21-06-18屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-24 21-06-18屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/2020-08-24 21-17-47屏幕截图.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/2020-08-24 21-17-47屏幕截图.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/after shrink.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/after shrink.png -------------------------------------------------------------------------------- /stupid_yellow_car/images/before shrink.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LeoDuhz/Advanced_Robotics/ad45a6f42f85c779b33408b2e409324e4697717a/stupid_yellow_car/images/before shrink.png --------------------------------------------------------------------------------