├── srv ├── GetNumOfWaypoints.srv ├── SaveWaypoints.srv ├── AddNewWaypoint.srv ├── GetChargerByName.srv ├── GetWaypointByName.srv └── GetWaypointByIndex.srv ├── msg └── Waypoint.msg ├── media ├── map.png ├── toolbar.png ├── map_tools.png ├── waypoint.png ├── add_waypoint.png └── wpb_home_nav.png ├── icons └── classes │ ├── AddCharger.png │ └── AddWaypoint.png ├── scripts ├── install_for_indigo.sh ├── install_for_noetic.sh ├── install_for_kinetic.sh └── install_for_melodic.sh ├── launch ├── add_waypoint_monitor.launch ├── add_waypoint_app.launch ├── add_waypoint.launch ├── add_waypoint_ai.launch ├── add_waypoint_mani.launch ├── add_waypoint_wpr1.launch ├── add_waypoint_wpr2.launch ├── add_waypoint_wpv3.launch ├── add_waypoint_wpv4.launch ├── add_waypoint_simulation.launch ├── add_waypoint_warehousing.launch ├── wpr1_nav_test.launch ├── wpv3_nav_test.launch ├── wpr1_nav_remote.launch ├── wpb_ai_nav_test.launch ├── wpb_home_nav_test.launch ├── wpb_mani_nav_test.launch └── wpb_home_nav_remote.launch ├── include ├── UDPClient.h ├── UDPServer.h ├── add_charger_tool.h └── add_waypoint_tool.h ├── waterplus_map_tools_plugin.xml ├── package.xml ├── README.md ├── src ├── charger_get_position.cpp ├── wp_saver.cpp ├── network │ ├── UDPClient.c │ └── UDPServer.c ├── wp_nav_odom_report.cpp ├── set_pose_from_waypoint.cpp ├── pose_navi_server.cpp ├── wp_set_pose.cpp ├── add_charger_tool.cpp ├── add_waypoint_tool.cpp ├── wp_navi_server.cpp ├── wp_nav_test.cpp ├── wp_nav_remote.cpp ├── wp_manager.cpp └── wp_edit_node.cpp ├── CMakeLists.txt ├── rviz ├── addwaypoint.rviz ├── editwaypoints.rviz └── nav.rviz ├── meshes └── charger.dae └── LICENSE /srv/GetNumOfWaypoints.srv: -------------------------------------------------------------------------------- 1 | --- 2 | int32 num -------------------------------------------------------------------------------- /srv/SaveWaypoints.srv: -------------------------------------------------------------------------------- 1 | string filename 2 | --- -------------------------------------------------------------------------------- /msg/Waypoint.msg: -------------------------------------------------------------------------------- 1 | string frame_id 2 | string name 3 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /srv/AddNewWaypoint.srv: -------------------------------------------------------------------------------- 1 | string name 2 | geometry_msgs/Pose pose 3 | --- 4 | bool result -------------------------------------------------------------------------------- /srv/GetChargerByName.srv: -------------------------------------------------------------------------------- 1 | string name 2 | --- 3 | string name 4 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /srv/GetWaypointByName.srv: -------------------------------------------------------------------------------- 1 | string name 2 | --- 3 | string name 4 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /media/map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6-robot/waterplus_map_tools/HEAD/media/map.png -------------------------------------------------------------------------------- /srv/GetWaypointByIndex.srv: -------------------------------------------------------------------------------- 1 | int32 index 2 | --- 3 | string name 4 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /media/toolbar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6-robot/waterplus_map_tools/HEAD/media/toolbar.png -------------------------------------------------------------------------------- /media/map_tools.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6-robot/waterplus_map_tools/HEAD/media/map_tools.png -------------------------------------------------------------------------------- /media/waypoint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6-robot/waterplus_map_tools/HEAD/media/waypoint.png -------------------------------------------------------------------------------- /media/add_waypoint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6-robot/waterplus_map_tools/HEAD/media/add_waypoint.png -------------------------------------------------------------------------------- /media/wpb_home_nav.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6-robot/waterplus_map_tools/HEAD/media/wpb_home_nav.png -------------------------------------------------------------------------------- /icons/classes/AddCharger.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6-robot/waterplus_map_tools/HEAD/icons/classes/AddCharger.png -------------------------------------------------------------------------------- /icons/classes/AddWaypoint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/6-robot/waterplus_map_tools/HEAD/icons/classes/AddWaypoint.png -------------------------------------------------------------------------------- /scripts/install_for_indigo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | sudo apt-get update 3 | sudo apt-get install ros-indigo-map-server 4 | sudo apt-get install ros-indigo-navigation -------------------------------------------------------------------------------- /scripts/install_for_noetic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | sudo apt-get update 3 | sudo apt-get install ros-noetic-map-server 4 | sudo apt-get install ros-noetic-navigation -------------------------------------------------------------------------------- /scripts/install_for_kinetic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | sudo apt-get update 3 | sudo apt-get install ros-kinetic-map-server 4 | sudo apt-get install ros-kinetic-navigation -------------------------------------------------------------------------------- /scripts/install_for_melodic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | sudo apt-get update 3 | sudo apt-get install ros-melodic-map-server 4 | sudo apt-get install ros-melodic-navigation -------------------------------------------------------------------------------- /launch/add_waypoint_monitor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/add_waypoint_app.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /include/UDPClient.h: -------------------------------------------------------------------------------- 1 | #ifndef _EXS_UDPCLIENT_H 2 | #define _EXS_UDPCLIENT_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | void InitUDPClient(char* inServerIP, int inPort); 11 | void UDPClientSend(unsigned char* inData,int inLen); 12 | void SendRobotState(int inID, int inState, float inX, float inY, float inAngle); 13 | #endif 14 | -------------------------------------------------------------------------------- /waterplus_map_tools_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Tool for adding waypoints on the ground plane in rviz. 7 | 8 | 9 | 12 | 13 | Tool for adding chargers on the ground plane in rviz. 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/add_waypoint.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/add_waypoint_ai.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/add_waypoint_mani.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/add_waypoint_wpr1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/add_waypoint_wpr2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/add_waypoint_wpv3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/add_waypoint_wpv4.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/add_waypoint_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/add_waypoint_warehousing.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /include/UDPServer.h: -------------------------------------------------------------------------------- 1 | #ifndef _EXS_UDPSERVER_H 2 | #define _EXS_UDPSERVER_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define bool char 12 | #define true 1 13 | #define false 0 14 | 15 | #define CTRL_STOP 0x00 16 | #define CTRL_MOVETO_POS 0x01 17 | #define CTRL_MOVETO_NAME 0x02 18 | #define CTRL_GRAB 0x03 19 | #define CTRL_PASS 0x04 20 | 21 | typedef struct ST_Ctrl 22 | { 23 | int ctrl; 24 | float x; 25 | float y; 26 | float angle; 27 | char wp_name; 28 | }ST_Ctrl; 29 | 30 | void InitUDPServer(int inPort); 31 | int UDPServerLoop(); 32 | bool GetCtrlCmd(ST_Ctrl* inCtrl); 33 | #endif 34 | -------------------------------------------------------------------------------- /include/add_charger_tool.h: -------------------------------------------------------------------------------- 1 | #ifndef ADD_CHARGER_TOOL_H 2 | #define ADD_CHARGER_TOOL_H 3 | 4 | #ifndef Q_MOC_RUN 5 | #include 6 | #include 7 | #include "rviz/default_plugin/tools/pose_tool.h" 8 | #endif 9 | 10 | namespace rviz 11 | { 12 | class Arrow; 13 | class DisplayContext; 14 | class StringProperty; 15 | 16 | class AddChargerTool: public rviz::PoseTool 17 | { 18 | Q_OBJECT 19 | public: 20 | AddChargerTool(); 21 | ~AddChargerTool(); 22 | 23 | virtual void onInitialize(); 24 | 25 | protected: 26 | virtual void onPoseSet(double x, double y, double theta); 27 | 28 | private Q_SLOTS: 29 | void updateTopic(); 30 | 31 | private: 32 | ros::NodeHandle nh_; 33 | ros::Publisher pub_; 34 | ros::ServiceClient cliGetChName; 35 | StringProperty* topic_property_; 36 | }; 37 | 38 | } 39 | 40 | #endif // ADD_CHARGER_TOOL_H -------------------------------------------------------------------------------- /include/add_waypoint_tool.h: -------------------------------------------------------------------------------- 1 | #ifndef ADD_WAYPOINT_TOOL_H 2 | #define ADD_WAYPOINT_TOOL_H 3 | 4 | #ifndef Q_MOC_RUN 5 | #include 6 | #include 7 | #include "rviz/default_plugin/tools/pose_tool.h" 8 | #endif 9 | 10 | namespace rviz 11 | { 12 | class Arrow; 13 | class DisplayContext; 14 | class StringProperty; 15 | 16 | class AddWaypointTool: public rviz::PoseTool 17 | { 18 | Q_OBJECT 19 | public: 20 | AddWaypointTool(); 21 | ~AddWaypointTool(); 22 | 23 | virtual void onInitialize(); 24 | 25 | protected: 26 | virtual void onPoseSet(double x, double y, double theta); 27 | 28 | private Q_SLOTS: 29 | void updateTopic(); 30 | 31 | private: 32 | ros::NodeHandle nh_; 33 | ros::Publisher pub_; 34 | ros::ServiceClient cliGetWPName; 35 | StringProperty* topic_property_; 36 | }; 37 | 38 | } 39 | 40 | #endif // ADD_WAYPOINT_TOOL_H -------------------------------------------------------------------------------- /launch/wpr1_nav_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /launch/wpv3_nav_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /launch/wpr1_nav_remote.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | waterplus_map_tools 3 | 0.1.0 4 | The waterplus_map_tools package 5 | 6 | Zhang Wanjie 7 | 8 | http://www.6-robot.com 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | message_generation 15 | std_msgs 16 | geometry_msgs 17 | roscpp 18 | rviz 19 | visualization_msgs 20 | interactive_markers 21 | tf 22 | actionlib 23 | tinyxml 24 | cmake_modules 25 | 26 | std_msgs 27 | geometry_msgs 28 | roscpp 29 | message_runtime 30 | rviz 31 | visualization_msgs 32 | interactive_markers 33 | tf 34 | actionlib 35 | tinyxml 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MapTools工具 2 | 3 | ## 使用步骤 4 | 5 | 1. 安装ROS(indigo/kinetic/melodic/noetic) 6 | 2. 配置好开发环境. [配置方法](http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment) 7 | 3. 获取源码: 8 | ``` 9 | cd ~/catkin_ws/src/ 10 | git clone https://github.com/6-robot/waterplus_map_tools.git 11 | ``` 12 | 4. 安装依赖项(根据ROS版本选择其中一个): 13 | ``` 14 | ~/catkin_ws/src/waterplus_map_tools/scripts/install_for_indigo.sh 15 | ``` 16 | ``` 17 | ~/catkin_ws/src/waterplus_map_tools/scripts/install_for_kinetic.sh 18 | ``` 19 | ``` 20 | ~/catkin_ws/src/waterplus_map_tools/scripts/install_for_melodic.sh 21 | ``` 22 | ``` 23 | ~/catkin_ws/src/waterplus_map_tools/scripts/install_for_noetic.sh 24 | ``` 25 | 5. 编译 26 | ``` 27 | cd ~/catkin_ws 28 | catkin_make 29 | ``` 30 | 31 | ## 平台介绍 32 | MapTools工具是[北京六部工坊科技有限公司](http://www.6-robot.com)为旗下WP系列机器人快速设置地图航点所设计的辅助工具,具有操作简单,效果直观的优点。目前支持启智ROS,启智AI,启智MANI,启程3,启程4和启明1等型号的机器人. 33 | ![Nav pic](./media/wpb_home_nav.png) 34 | 35 | ## 操作方法 36 | 37 | ### 1. 打开地图 38 | 启智ROS: 39 | ``` 40 | roslaunch waterplus_map_tools add_waypoint.launch 41 | ``` 42 | 启智AI: 43 | ``` 44 | roslaunch waterplus_map_tools add_waypoint_ai.launch 45 | ``` 46 | 启智MANI: 47 | ``` 48 | roslaunch waterplus_map_tools add_waypoint_mani.launch 49 | ``` 50 | 启程3: 51 | ``` 52 | roslaunch waterplus_map_tools add_waypoint_wpv3.launch 53 | ``` 54 | 启明1: 55 | ``` 56 | roslaunch waterplus_map_tools add_waypoint_wpr1.launch 57 | ``` 58 | ![1 pic](./media/map.png) 59 | 60 | ### 2. 设置航点 61 | 在Rviz工具栏点击"Add Waypoint"按钮可在地图上设置航点。 62 | ![2 pic](./media/toolbar.png) 63 | ![3 pic](./media/add_waypoint.png) 64 | ![4 pic](./media/waypoint.png) 65 | ![MapTools pic](./media/map_tools.png) 66 | 67 | ### 3. 保存航点 68 | 航点设置完毕后,使用如下指令保存航点: 69 | ``` 70 | rosrun waterplus_map_tools wp_saver 71 | ``` 72 | 73 | ### 4. 航点遍历 74 | 航点设置完毕后,可以使用如下指令让机器人将设置的航点逐个遍历: 75 | ``` 76 | rosrun waterplus_map_tools wp_nav_test 77 | ``` 78 | -------------------------------------------------------------------------------- /src/charger_get_position.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* @author Zhang Wanjie */ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | int main(int argc, char** argv) 44 | { 45 | ros::init(argc, argv, "charger_get_position"); 46 | 47 | ros::NodeHandle nh; 48 | ros::ServiceClient cliGetChName = nh.serviceClient("waterplus/get_charger_name"); 49 | 50 | waterplus_map_tools::GetChargerByName srvN; 51 | srvN.request.name = "c1"; 52 | if (cliGetChName.call(srvN)) 53 | { 54 | std::string name = srvN.response.name; 55 | float x = srvN.response.pose.position.x; 56 | float y = srvN.response.pose.position.y; 57 | ROS_INFO("Get_charger_name: name = %s (%.2f,%.2f)", srvN.request.name.c_str(),x,y); 58 | } 59 | else 60 | { 61 | ROS_ERROR("No charger named ( %s )", srvN.request.name.c_str()); 62 | } 63 | 64 | ros::spinOnce(); 65 | 66 | return 0; 67 | } -------------------------------------------------------------------------------- /src/wp_saver.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* @author Zhang Wanjie */ 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | int main(int argc, char** argv) 41 | { 42 | ros::init(argc, argv, "wp_saver"); 43 | 44 | ros::NodeHandle nh; 45 | ros::ServiceClient cliSave = nh.serviceClient("waterplus/save_waypoints"); 46 | waterplus_map_tools::SaveWaypoints srvS; 47 | 48 | std::string strSaveFile; 49 | char const* home = getenv("HOME"); 50 | strSaveFile = home; 51 | strSaveFile += "/waypoints.xml"; 52 | srvS.request.filename = strSaveFile; 53 | 54 | for(int i=1; i> 8); 39 | inDest[1] = (unsigned char )(inVal); 40 | } 41 | 42 | void Int32ToBytes(int inVal,unsigned char* inDest) 43 | { 44 | inDest[0] = (unsigned char )(inVal >> 24); 45 | inDest[1] = (unsigned char )(inVal >> 16); 46 | inDest[2] = (unsigned char )(inVal >> 8); 47 | inDest[3] = (unsigned char )(inVal); 48 | } 49 | 50 | void FloatToBytes(float inVal,unsigned char* inDest) 51 | { 52 | memcpy(inDest,(unsigned char*)&inVal,sizeof(float)); 53 | } 54 | 55 | 56 | void ReportResult(int inX,int inY,int inSum) 57 | { 58 | arSendBuf[0] = 0x55; 59 | arSendBuf[1] = 0xaa; 60 | arSendBuf[2] = 13; //len 61 | arSendBuf[3] = 0x00; //flag 62 | //x 63 | WordToBytes(inX,&arSendBuf[4]); 64 | //y 65 | WordToBytes(inY,&arSendBuf[6]); 66 | //sum 67 | Int32ToBytes(inSum,&arSendBuf[8]); 68 | 69 | arSendBuf[arSendBuf[2]-1] = 0; 70 | int i=0; 71 | for(i=0;i 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /launch/wpb_home_nav_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /launch/wpb_mani_nav_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /launch/wpb_home_nav_remote.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /src/wp_nav_odom_report.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* @author Zhang Wanjie */ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | extern "C" { 41 | #include "UDPClient.h" 42 | } 43 | 44 | static int Robot_ID = 1; //机器人ID 45 | 46 | int main(int argc, char** argv) 47 | { 48 | ros::init(argc, argv, "wp_nav_odom_report"); 49 | InitUDPClient((char*)"192.168.1.110", 20180); //服务器IP地址 50 | 51 | ros::NodeHandle nh; 52 | ros::Rate r(10.0); 53 | 54 | tf::TransformListener listener; 55 | tf::StampedTransform transform; 56 | 57 | while(ros::ok()) 58 | { 59 | try 60 | { 61 | listener.waitForTransform("map","base_footprint", ros::Time(0), ros::Duration(10.0) ); 62 | listener.lookupTransform("map","base_footprint", ros::Time(0), transform); 63 | } 64 | catch (tf::TransformException &ex) 65 | { 66 | ROS_ERROR("[lookupTransform] %s",ex.what()); 67 | break; 68 | } 69 | float tx = transform.getOrigin().x(); 70 | float ty = transform.getOrigin().y(); 71 | tf::Stamped p = tf::Stamped(tf::Pose(transform.getRotation() , tf::Point(tx, ty, 0.0)), ros::Time::now(), "map"); 72 | geometry_msgs::PoseStamped robot_pos; 73 | tf::poseStampedTFToMsg(p, robot_pos); 74 | 75 | tf::Quaternion quat(robot_pos.pose.orientation.x,robot_pos.pose.orientation.y,robot_pos.pose.orientation.z,robot_pos.pose.orientation.w); 76 | double roll,pitch,yaw; 77 | tf::Matrix3x3(quat).getRPY(roll,pitch,yaw); 78 | ROS_WARN("[Robot Pos]( %.2f , %.2f ) - %.2f" , robot_pos.pose.position.x, robot_pos.pose.position.y, yaw); 79 | 80 | SendRobotState(Robot_ID, 1, robot_pos.pose.position.x, robot_pos.pose.position.y, yaw); 81 | 82 | ros::spinOnce(); 83 | r.sleep(); 84 | } 85 | 86 | return 0; 87 | } -------------------------------------------------------------------------------- /src/set_pose_from_waypoint.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2025-2035, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /*!****************************************************************** 35 | @author ZhangWanjie 36 | ********************************************************************/ 37 | #include 38 | #include 39 | #include 40 | #include "waterplus_map_tools/GetWaypointByName.h" 41 | 42 | int main(int argc, char** argv) 43 | { 44 | setlocale(LC_ALL,""); 45 | // 初始化ROS节点 46 | ros::init(argc, argv, "set_pose_from_waypoint_node"); 47 | ros::NodeHandle nh; 48 | 49 | // 1. 从命令行参数获取航点名称 50 | std::string waypoint_name; 51 | if (argc < 2) 52 | { 53 | return 1; 54 | } 55 | waypoint_name = argv[1]; // argv[0] 是程序名, argv[1] 是第一个参数 56 | 57 | // 2. 创建服务客户端，并等待服务启动 58 | ros::ServiceClient client = nh.serviceClient("/waterplus/get_waypoint_name"); 59 | ROS_INFO("等待服务 '/waterplus/get_waypoint_name' 启动..."); 60 | client.waitForExistence(); // 等待服务启动 61 | 62 | // 3. 准备并发送服务请求 63 | waterplus_map_tools::GetWaypointByName srv; 64 | srv.request.name = waypoint_name; 65 | 66 | if (client.call(srv)) 67 | { 68 | // 4. 创建到 /initialpose 话题的发布者 69 | ros::Publisher initial_pose_pub = nh.advertise("/initialpose", 1, true); 70 | 71 | // 5. 构建 geometry_msgs/PoseWithCovarianceStamped 消息 72 | geometry_msgs::PoseWithCovarianceStamped initial_pose_msg; 73 | initial_pose_msg.header.stamp = ros::Time::now(); 74 | initial_pose_msg.header.frame_id = "map"; // 初始位姿通常在 "map" 坐标系下 75 | 76 | // 填充从服务获取的位姿 77 | initial_pose_msg.pose.pose = srv.response.pose; 78 | 79 | // 填充协方差矩阵 (covariance) 80 | initial_pose_msg.pose.covariance[0] = 0.25; // x的方差 (0.5m^2) 81 | initial_pose_msg.pose.covariance[7] = 0.25; // y的方差 (0.5m^2) 82 | initial_pose_msg.pose.covariance[35] = 0.0685; // yaw角的方差 (约0.26弧度^2) 83 | 84 | // 稍作等待，确保发布者已经连接到订阅者 85 | ros::Duration(1.0).sleep(); 86 | 87 | // 6. 发布消息 88 | initial_pose_pub.publish(initial_pose_msg); 89 | 90 | ROS_WARN("自动定位机器人到航点位置: %s", waypoint_name.c_str()); 91 | } 92 | else 93 | { 94 | ROS_ERROR("调用 /waterplus/get_waypoint_name 服务失败！ 航点 '%s' 是否未设置？", waypoint_name.c_str()); 95 | return 1; 96 | } 97 | 98 | // 给发布足够的时间，并处理任何待处理的ROS回调 99 | ros::spinOnce(); 100 | ros::Duration(1.0).sleep(); 101 | 102 | return 0; 103 | } -------------------------------------------------------------------------------- /src/pose_navi_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /*!****************************************************************** 35 | @author ZhangWanjie 36 | ********************************************************************/ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | static bool bNewCmd = false; 48 | static geometry_msgs::Pose goal_pose; 49 | static ros::Publisher result_pub; 50 | static std_msgs::String result_msg; 51 | 52 | typedef actionlib::SimpleActionClient MoveBaseClient; 53 | 54 | void NaviPoseCB(const geometry_msgs::Pose::ConstPtr &msg) 55 | { 56 | goal_pose = *msg; 57 | bNewCmd = true; 58 | } 59 | 60 | int main(int argc, char** argv) 61 | { 62 | ros::init(argc, argv, "pose_navi_server"); 63 | 64 | ros::NodeHandle n; 65 | ros::Subscriber navi_name_sub = n.subscribe("waterplus/navi_pose", 10, NaviPoseCB); 66 | result_pub = n.advertise("waterplus/navi_result", 10); 67 | 68 | MoveBaseClient ac("move_base", true); 69 | move_base_msgs::MoveBaseGoal goal; 70 | 71 | ros::Rate r(30); 72 | 73 | while(ros::ok()) 74 | { 75 | if(bNewCmd) 76 | { 77 | //wait for the action server to come up 78 | while(!ac.waitForServer(ros::Duration(5.0))) 79 | { 80 | if(!ros::ok()) 81 | break; 82 | ROS_INFO("Waiting for the move_base action server to come up"); 83 | } 84 | 85 | goal.target_pose.header.frame_id = "map"; 86 | goal.target_pose.header.stamp = ros::Time::now(); 87 | goal.target_pose.pose = goal_pose; 88 | ac.sendGoal(goal); 89 | ac.waitForResult(); 90 | 91 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 92 | { 93 | ROS_INFO("Arrived at Pose ( %.2f , %.2f ) !",goal_pose.position.x, goal_pose.position.y); 94 | } 95 | else 96 | ROS_WARN("Failed to get to WayPoint ( %.2f , %.2f )..." ,goal_pose.position.x, goal_pose.position.y); 97 | result_msg.data = "done"; 98 | result_pub.publish(result_msg); 99 | bNewCmd = false; 100 | } 101 | ros::spinOnce(); 102 | r.sleep(); 103 | } 104 | 105 | return 0; 106 | } -------------------------------------------------------------------------------- /src/wp_set_pose.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2025-2035, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /*!****************************************************************** 35 | @author ZhangWanjie 36 | ********************************************************************/ 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include "waterplus_map_tools/GetWaypointByName.h" 42 | 43 | // 全局变量，用于发布者和服务客户端 44 | ros::Publisher initial_pose_pub; 45 | ros::ServiceClient client; 46 | 47 | // 回调函数，处理来自 /waterplus/set_pose 话题的消息 48 | void setPoseCallback(const std_msgs::String::ConstPtr& msg) 49 | { 50 | std::string waypoint_name = msg->data; 51 | ROS_INFO("接收到航点名称: %s", waypoint_name.c_str()); 52 | 53 | // 准备并发送服务请求 54 | waterplus_map_tools::GetWaypointByName srv; 55 | srv.request.name = waypoint_name; 56 | 57 | if (client.call(srv)) 58 | { 59 | // 构建 geometry_msgs/PoseWithCovarianceStamped 消息 60 | geometry_msgs::PoseWithCovarianceStamped initial_pose_msg; 61 | initial_pose_msg.header.stamp = ros::Time::now(); 62 | initial_pose_msg.header.frame_id = "map"; // 初始位姿通常在 "map" 坐标系下 63 | 64 | // 填充从服务获取的位姿 65 | initial_pose_msg.pose.pose = srv.response.pose; 66 | 67 | // 填充协方差矩阵 (covariance) 68 | initial_pose_msg.pose.covariance[0] = 0.25; // x的方差 (0.5m^2) 69 | initial_pose_msg.pose.covariance[7] = 0.25; // y的方差 (0.5m^2) 70 | initial_pose_msg.pose.covariance[35] = 0.0685; // yaw角的方差 (约0.26弧度^2) 71 | 72 | // 稍作等待，确保发布者已经连接到订阅者 73 | ros::Duration(1.0).sleep(); 74 | 75 | // 发布消息 76 | initial_pose_pub.publish(initial_pose_msg); 77 | 78 | ROS_WARN("将机器人定位到航点位置: %s", waypoint_name.c_str()); 79 | } 80 | else 81 | { 82 | ROS_ERROR("调用 /waterplus/get_waypoint_name 服务失败！ 航点 '%s' 是否未设置？", waypoint_name.c_str()); 83 | } 84 | } 85 | 86 | int main(int argc, char** argv) 87 | { 88 | setlocale(LC_ALL,""); 89 | // 初始化ROS节点 90 | ros::init(argc, argv, "wp_set_pose"); 91 | ros::NodeHandle nh; 92 | 93 | // 创建到 /initialpose 话题的发布者 94 | initial_pose_pub = nh.advertise("/initialpose", 1, true); 95 | 96 | // 创建服务客户端，并等待服务启动 97 | client = nh.serviceClient("/waterplus/get_waypoint_name"); 98 | ROS_INFO("等待服务 '/waterplus/get_waypoint_name' 启动..."); 99 | client.waitForExistence(); // 等待服务启动 100 | 101 | // 创建 /waterplus/set_pose 话题的订阅者 102 | ros::Subscriber sub = nh.subscribe("/waterplus/set_pose", 1, setPoseCallback); 103 | 104 | ROS_INFO("节点已准备就绪，等待来自'/waterplus/set_pose'的航点名称..."); 105 | 106 | // 保持节点运行，处理回调 107 | ros::spin(); 108 | 109 | return 0; 110 | } -------------------------------------------------------------------------------- /src/add_charger_tool.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* @author Zhang Wanjie */ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include "rviz/display_context.h" 41 | #include "rviz/properties/string_property.h" 42 | #include "add_charger_tool.h" 43 | 44 | static int nChargerCount = 0; 45 | 46 | namespace rviz 47 | { 48 | AddChargerTool::AddChargerTool() 49 | { 50 | shortcut_key_ = 'c'; 51 | topic_property_ = new StringProperty( "Topic", "/waterplus/add_charger","The topic on which to add new charger.",getPropertyContainer(), SLOT( updateTopic() ), this ); 52 | } 53 | 54 | AddChargerTool::~AddChargerTool() 55 | { 56 | } 57 | 58 | void AddChargerTool::onInitialize() 59 | { 60 | PoseTool::onInitialize(); 61 | setName( "Add Charger" ); 62 | updateTopic(); 63 | } 64 | 65 | void AddChargerTool::updateTopic() 66 | { 67 | pub_ = nh_.advertise( topic_property_->getStdString(), 1); 68 | cliGetChName = nh_.serviceClient("/waterplus/get_charger_name"); 69 | } 70 | 71 | void AddChargerTool::onPoseSet(double x, double y, double theta) 72 | { 73 | std::string fixed_frame = context_->getFixedFrame().toStdString(); 74 | tf::Quaternion quat; 75 | quat.setRPY(0.0, 0.0, theta); 76 | tf::Stamped p = tf::Stamped(tf::Pose(quat, tf::Point(x, y, 0.0)), ros::Time::now(), fixed_frame); 77 | geometry_msgs::PoseStamped new_pos; 78 | tf::poseStampedTFToMsg(p, new_pos); 79 | ROS_INFO("Add new charger: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(), 80 | new_pos.pose.position.x, new_pos.pose.position.y, new_pos.pose.position.z, 81 | new_pos.pose.orientation.x, new_pos.pose.orientation.y, new_pos.pose.orientation.z, new_pos.pose.orientation.w, theta); 82 | waterplus_map_tools::Waypoint new_charger; 83 | 84 | nChargerCount ++; 85 | std::ostringstream stringStream; 86 | stringStream << "c" << nChargerCount; 87 | std::string strNewName = stringStream.str(); 88 | 89 | waterplus_map_tools::GetChargerByName srvN; 90 | srvN.request.name = strNewName; 91 | while (cliGetChName.call(srvN)) 92 | { 93 | nChargerCount ++; 94 | std::ostringstream stringStream; 95 | stringStream << "c" << nChargerCount; 96 | strNewName = stringStream.str(); 97 | srvN.request.name = strNewName; 98 | } 99 | ROS_WARN("New charger name = %s",strNewName.c_str()); 100 | new_charger.name = strNewName; 101 | 102 | new_charger.pose = new_pos.pose; 103 | pub_.publish(new_charger); 104 | } 105 | } 106 | 107 | #include 108 | PLUGINLIB_EXPORT_CLASS(rviz::AddChargerTool,rviz::Tool) 109 | -------------------------------------------------------------------------------- /src/add_waypoint_tool.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* @author Zhang Wanjie */ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include "rviz/display_context.h" 41 | #include "rviz/properties/string_property.h" 42 | #include "add_waypoint_tool.h" 43 | 44 | static int nWaypointCount = 0; 45 | 46 | namespace rviz 47 | { 48 | AddWaypointTool::AddWaypointTool() 49 | { 50 | shortcut_key_ = 'a'; 51 | topic_property_ = new StringProperty( "Topic", "/waterplus/add_waypoint","The topic on which to add new waypoints.",getPropertyContainer(), SLOT( updateTopic() ), this ); 52 | } 53 | 54 | AddWaypointTool::~AddWaypointTool() 55 | { 56 | } 57 | 58 | void AddWaypointTool::onInitialize() 59 | { 60 | PoseTool::onInitialize(); 61 | setName( "Add Waypoint" ); 62 | updateTopic(); 63 | } 64 | 65 | void AddWaypointTool::updateTopic() 66 | { 67 | pub_ = nh_.advertise( topic_property_->getStdString(), 1); 68 | cliGetWPName = nh_.serviceClient("/waterplus/get_waypoint_name"); 69 | } 70 | 71 | void AddWaypointTool::onPoseSet(double x, double y, double theta) 72 | { 73 | std::string fixed_frame = context_->getFixedFrame().toStdString(); 74 | tf::Quaternion quat; 75 | quat.setRPY(0.0, 0.0, theta); 76 | tf::Stamped p = tf::Stamped(tf::Pose(quat, tf::Point(x, y, 0.0)), ros::Time::now(), fixed_frame); 77 | geometry_msgs::PoseStamped new_pos; 78 | tf::poseStampedTFToMsg(p, new_pos); 79 | ROS_INFO("Add new waypoint: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(), 80 | new_pos.pose.position.x, new_pos.pose.position.y, new_pos.pose.position.z, 81 | new_pos.pose.orientation.x, new_pos.pose.orientation.y, new_pos.pose.orientation.z, new_pos.pose.orientation.w, theta); 82 | waterplus_map_tools::Waypoint new_waypoint; 83 | 84 | nWaypointCount ++; 85 | std::ostringstream stringStream; 86 | stringStream << nWaypointCount; 87 | std::string strNewName = stringStream.str(); 88 | 89 | waterplus_map_tools::GetWaypointByName srvN; 90 | srvN.request.name = strNewName; 91 | while (cliGetWPName.call(srvN)) 92 | { 93 | nWaypointCount ++; 94 | std::ostringstream stringStream; 95 | stringStream << nWaypointCount; 96 | strNewName = stringStream.str(); 97 | srvN.request.name = strNewName; 98 | } 99 | ROS_WARN("New waypoint name = %s",strNewName.c_str()); 100 | new_waypoint.name = strNewName; 101 | 102 | new_waypoint.pose = new_pos.pose; 103 | pub_.publish(new_waypoint); 104 | } 105 | } 106 | 107 | #include 108 | PLUGINLIB_EXPORT_CLASS(rviz::AddWaypointTool,rviz::Tool) 109 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waterplus_map_tools) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation rviz roscpp geometry_msgs std_msgs tf visualization_msgs interactive_markers move_base_msgs actionlib cmake_modules ) 5 | 6 | add_service_files(FILES 7 | SaveWaypoints.srv 8 | AddNewWaypoint.srv 9 | GetNumOfWaypoints.srv 10 | GetWaypointByIndex.srv 11 | GetWaypointByName.srv 12 | GetChargerByName.srv 13 | ) 14 | 15 | add_message_files(FILES 16 | Waypoint.msg 17 | ) 18 | 19 | generate_messages(DEPENDENCIES 20 | std_msgs 21 | geometry_msgs 22 | ) 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS message_runtime 26 | INCLUDE_DIRS include 27 | LIBRARIES waterplus_map_tools 28 | DEPENDS TinyXML 29 | ) 30 | 31 | 32 | include_directories(include ${catkin_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS}) 33 | link_directories(${catkin_LIBRARY_DIRS}) 34 | 35 | set(CMAKE_AUTOMOC ON) 36 | if(rviz_QT_VERSION VERSION_LESS "5") 37 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 38 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 39 | include(${QT_USE_FILE}) 40 | else() 41 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 42 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 43 | set(QT_LIBRARIES Qt5::Widgets) 44 | endif() 45 | add_definitions(-DQT_NO_KEYWORDS) 46 | 47 | find_package(TinyXML REQUIRED) 48 | 49 | add_library(${PROJECT_NAME} 50 | include/add_waypoint_tool.h 51 | src/add_waypoint_tool.cpp 52 | include/add_charger_tool.h 53 | src/add_charger_tool.cpp 54 | ) 55 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 56 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 57 | 58 | ## Install rules 59 | 60 | install(TARGETS 61 | ${PROJECT_NAME} 62 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 65 | ) 66 | 67 | install(FILES 68 | waterplus_map_tools_plugin.xml 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 70 | 71 | add_executable(wp_manager 72 | src/wp_manager.cpp 73 | ) 74 | add_dependencies(wp_manager ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 75 | target_link_libraries(wp_manager 76 | ${catkin_LIBRARIES} ${TinyXML_LIBRARIES} 77 | ) 78 | 79 | add_executable(wp_edit_node 80 | src/wp_edit_node.cpp 81 | ) 82 | add_dependencies(wp_edit_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 83 | target_link_libraries(wp_edit_node 84 | ${catkin_LIBRARIES} ${TinyXML_LIBRARIES} 85 | ) 86 | 87 | add_executable(wp_nav_test 88 | src/wp_nav_test.cpp 89 | ) 90 | add_dependencies(wp_nav_test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 91 | target_link_libraries(wp_nav_test 92 | ${catkin_LIBRARIES} 93 | ) 94 | 95 | add_executable(wp_saver 96 | src/wp_saver.cpp 97 | ) 98 | add_dependencies(wp_saver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 99 | target_link_libraries(wp_saver 100 | ${catkin_LIBRARIES} 101 | ) 102 | 103 | add_executable(wp_nav_remote 104 | src/wp_nav_remote.cpp 105 | src/network/UDPServer.c 106 | ) 107 | add_dependencies(wp_nav_remote ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 108 | target_link_libraries(wp_nav_remote 109 | ${catkin_LIBRARIES} 110 | ) 111 | 112 | add_executable(wp_nav_odom_report 113 | src/wp_nav_odom_report.cpp 114 | src/network/UDPClient.c 115 | ) 116 | add_dependencies(wp_nav_odom_report ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 117 | target_link_libraries(wp_nav_odom_report 118 | ${catkin_LIBRARIES} 119 | ) 120 | 121 | add_executable(charger_get_position 122 | src/charger_get_position.cpp 123 | ) 124 | add_dependencies(charger_get_position ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 125 | target_link_libraries(charger_get_position 126 | ${catkin_LIBRARIES} 127 | ) 128 | 129 | add_executable(wp_navi_server 130 | src/wp_navi_server.cpp 131 | ) 132 | add_dependencies(wp_navi_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | target_link_libraries(wp_navi_server 134 | ${catkin_LIBRARIES} 135 | ) 136 | 137 | add_executable(pose_navi_server 138 | src/pose_navi_server.cpp 139 | ) 140 | add_dependencies(pose_navi_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 141 | target_link_libraries(pose_navi_server 142 | ${catkin_LIBRARIES} 143 | ) 144 | 145 | add_executable(set_pose_from_waypoint 146 | src/set_pose_from_waypoint.cpp 147 | ) 148 | add_dependencies(set_pose_from_waypoint ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | target_link_libraries(set_pose_from_waypoint 150 | ${catkin_LIBRARIES} 151 | ) 152 | 153 | add_executable(wp_set_pose 154 | src/wp_set_pose.cpp 155 | ) 156 | add_dependencies(wp_set_pose ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 157 | target_link_libraries(wp_set_pose 158 | ${catkin_LIBRARIES} 159 | ) -------------------------------------------------------------------------------- /src/wp_navi_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /*!****************************************************************** 35 | @author ZhangWanjie 36 | ********************************************************************/ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | static bool bNewCmd = false; 48 | ros::ServiceClient cliGetWPName; 49 | static geometry_msgs::Pose wp_pose; 50 | static ros::Publisher result_pub; 51 | static std_msgs::String result_msg; 52 | 53 | typedef actionlib::SimpleActionClient MoveBaseClient; 54 | 55 | void NaviWaypointCB(const std_msgs::String::ConstPtr &msg) 56 | { 57 | waterplus_map_tools::GetWaypointByName srvN; 58 | srvN.request.name = msg->data; 59 | if (cliGetWPName.call(srvN)) 60 | { 61 | wp_pose = srvN.response.pose; 62 | std::string name = srvN.response.name; 63 | float x = srvN.response.pose.position.x; 64 | float y = srvN.response.pose.position.y; 65 | ROS_INFO("Get_wp_name: name = %s (%.2f,%.2f)", name.c_str(),x,y); 66 | } 67 | else 68 | { 69 | ROS_ERROR("Failed to call service get_waypoint_name"); 70 | } 71 | bNewCmd = true; 72 | } 73 | 74 | int main(int argc, char** argv) 75 | { 76 | ros::init(argc, argv, "wp_navi_server"); 77 | 78 | ros::NodeHandle n; 79 | ros::Subscriber navi_name_sub = n.subscribe("waterplus/navi_waypoint", 10, NaviWaypointCB); 80 | result_pub = n.advertise("waterplus/navi_result", 10); 81 | cliGetWPName = n.serviceClient("waterplus/get_waypoint_name"); 82 | 83 | MoveBaseClient ac("move_base", true); 84 | move_base_msgs::MoveBaseGoal goal; 85 | 86 | ros::Rate r(30); 87 | 88 | while(ros::ok()) 89 | { 90 | if(bNewCmd) 91 | { 92 | //wait for the action server to come up 93 | while(!ac.waitForServer(ros::Duration(5.0))) 94 | { 95 | if(!ros::ok()) 96 | break; 97 | ROS_INFO("Waiting for the move_base action server to come up"); 98 | } 99 | 100 | goal.target_pose.header.frame_id = "map"; 101 | goal.target_pose.header.stamp = ros::Time::now(); 102 | goal.target_pose.pose = wp_pose; 103 | ac.sendGoal(goal); 104 | ac.waitForResult(); 105 | 106 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 107 | { 108 | ROS_INFO("Arrived at WayPoint !"); 109 | result_msg.data = "done"; 110 | } 111 | else 112 | { 113 | ROS_WARN("Failed to get to WayPoint ..." ); 114 | result_msg.data = "failure"; 115 | } 116 | result_pub.publish(result_msg); 117 | bNewCmd = false; 118 | } 119 | ros::spinOnce(); 120 | r.sleep(); 121 | } 122 | 123 | return 0; 124 | } -------------------------------------------------------------------------------- /rviz/addwaypoint.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 | - /Map1 11 | - /Map1/Position1 12 | - /Waypoints1 13 | - /Chargers1 14 | Splitter Ratio: 0.5 15 | Tree Height: 767 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Add Waypoint1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.588679016 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: "" 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.0299999993 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 30 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 0.699999988 59 | Class: rviz/Map 60 | Color Scheme: map 61 | Draw Behind: false 62 | Enabled: true 63 | Name: Map 64 | Topic: /map 65 | Unreliable: false 66 | Use Timestamp: false 67 | Value: true 68 | - Class: rviz/Marker 69 | Enabled: true 70 | Marker Topic: /waypoints_marker 71 | Name: Waypoints 72 | Namespaces: 73 | marker_waypoints: true 74 | text: true 75 | Queue Size: 100 76 | Value: true 77 | - Class: rviz/Marker 78 | Enabled: true 79 | Marker Topic: /chargers_marker 80 | Name: Chargers 81 | Namespaces: 82 | marker_waypoints: true 83 | text: true 84 | Queue Size: 100 85 | Value: true 86 | Enabled: true 87 | Global Options: 88 | Background Color: 48; 48; 48 89 | Default Light: true 90 | Fixed Frame: map 91 | Frame Rate: 30 92 | Name: root 93 | Tools: 94 | - Class: rviz/Interact 95 | Hide Inactive Objects: true 96 | - Class: rviz/MoveCamera 97 | - Class: rviz/Select 98 | - Class: rviz/FocusCamera 99 | - Class: rviz/Measure 100 | - Class: rviz/SetInitialPose 101 | Topic: /initialpose 102 | - Class: rviz/SetGoal 103 | Topic: /move_base_simple/goal 104 | - Class: rviz/AddWaypoint 105 | Topic: /waterplus/add_waypoint 106 | Value: true 107 | Views: 108 | Current: 109 | Class: rviz/Orbit 110 | Distance: 5.88774872 111 | Enable Stereo Rendering: 112 | Stereo Eye Separation: 0.0599999987 113 | Stereo Focal Distance: 1 114 | Swap Stereo Eyes: false 115 | Value: false 116 | Focal Point: 117 | X: 0.487364948 118 | Y: -1.19927025 119 | Z: 0.804444849 120 | Focal Shape Fixed Size: true 121 | Focal Shape Size: 0.0500000007 122 | Invert Z Axis: false 123 | Name: Current View 124 | Near Clip Distance: 0.00999999978 125 | Pitch: 0.851830721 126 | Target Frame: 127 | Value: Orbit (rviz) 128 | Yaw: 3.1293335 129 | Saved: ~ 130 | Window Geometry: 131 | Displays: 132 | collapsed: true 133 | Height: 1056 134 | Hide Left Dock: true 135 | Hide Right Dock: false 136 | QMainWindow State: 000000ff00000000fd0000000400000000000001930000038efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000038e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000121000003f7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003f7000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000046fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000073f0000038e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 137 | Selection: 138 | collapsed: false 139 | Time: 140 | collapsed: false 141 | Tool Properties: 142 | collapsed: false 143 | Views: 144 | collapsed: false 145 | Width: 1855 146 | X: 65 147 | Y: 24 148 | -------------------------------------------------------------------------------- /src/network/UDPServer.c: -------------------------------------------------------------------------------- 1 | /*! 2 | @header UDPServer.c 3 | @abstract UDP通讯服务 4 | @author 张万杰 5 | */ 6 | #include "UDPServer.h" 7 | #include "UDPClient.h" 8 | 9 | static char buffer[1024]; 10 | static int sockfd,len; 11 | static struct sockaddr_in addr; 12 | static int addr_len; 13 | static pthread_t hThread; 14 | 15 | static int bFrameStart = 0; 16 | static int nParseIndex = 0; 17 | static int nLenToParse = 10; 18 | static unsigned char m_byteLast = 0; 19 | static unsigned char arParseBuf[1024]; 20 | 21 | static bool bNewCmd = false; 22 | static ST_Ctrl ctrl_recv; 23 | 24 | int BytesToWord(unsigned char* inBuf) 25 | { 26 | int res = 0; 27 | res = inBuf[0]; 28 | res <<= 8; 29 | res |= inBuf[1]; 30 | return res; 31 | } 32 | 33 | float BytesToFloat(unsigned char* inBuf) 34 | { 35 | float res = 0; 36 | memcpy((char*)&res,inBuf,sizeof(float)); 37 | return res; 38 | } 39 | 40 | void UDP_Server_ParseFrame(unsigned char* inBuf,int inLen) 41 | { 42 | printf("%d [UDP_Recv] ",inLen); 43 | int i =0; 44 | for(i=0;i= nLenToParse) 106 | { 107 | UDP_Server_ParseFrame((unsigned char*)arParseBuf,nLenToParse); 108 | bFrameStart = 0; 109 | nParseIndex = 0; 110 | m_byteLast = 0; 111 | } 112 | } 113 | } 114 | 115 | void *threadUDPServer_func() 116 | { 117 | printf ("[UDP_Server]threadUDPServer_func start...\n"); 118 | while(1) 119 | { 120 | bzero(buffer,sizeof(buffer)); 121 | len = recvfrom(sockfd,buffer,sizeof(buffer), 0 , (struct sockaddr *)&addr ,&addr_len); 122 | /*显示client端的网络地址*/ 123 | printf("receive %d from %s\n" , len , inet_ntoa( addr.sin_addr)); 124 | 125 | int i = 0; 126 | for(i=0;i 58 | Value: true 59 | - Alpha: 0.699999988 60 | Class: rviz/Map 61 | Color Scheme: map 62 | Draw Behind: false 63 | Enabled: true 64 | Name: Map 65 | Topic: /map 66 | Unreliable: false 67 | Use Timestamp: false 68 | Value: true 69 | - Class: rviz/InteractiveMarkers 70 | Enable Transparency: true 71 | Enabled: true 72 | Name: Waypoints 73 | Show Axes: false 74 | Show Descriptions: false 75 | Show Visual Aids: true 76 | Update Topic: /waypoints_move/update 77 | Value: true 78 | - Class: rviz/Marker 79 | Enabled: true 80 | Marker Topic: /text_marker 81 | Name: TextMarker 82 | Namespaces: 83 | text: true 84 | Queue Size: 100 85 | Value: true 86 | Enabled: true 87 | Global Options: 88 | Background Color: 48; 48; 48 89 | Default Light: true 90 | Fixed Frame: map 91 | Frame Rate: 30 92 | Name: root 93 | Tools: 94 | - Class: rviz/Interact 95 | Hide Inactive Objects: true 96 | - Class: rviz/MoveCamera 97 | - Class: rviz/Select 98 | - Class: rviz/FocusCamera 99 | - Class: rviz/Measure 100 | - Class: rviz/SetInitialPose 101 | Topic: /initialpose 102 | - Class: rviz/SetGoal 103 | Topic: /move_base_simple/goal 104 | - Class: rviz/AddWaypoint 105 | Topic: /waterplus/add_waypoint 106 | - Class: rviz/AddCharger 107 | Topic: /waterplus/add_charger 108 | Value: true 109 | Views: 110 | Current: 111 | Class: rviz/Orbit 112 | Distance: 11.0964251 113 | Enable Stereo Rendering: 114 | Stereo Eye Separation: 0.0599999987 115 | Stereo Focal Distance: 1 116 | Swap Stereo Eyes: false 117 | Value: false 118 | Focal Point: 119 | X: 0.130032673 120 | Y: -1.10872293 121 | Z: -0.59274745 122 | Focal Shape Fixed Size: true 123 | Focal Shape Size: 0.0500000007 124 | Invert Z Axis: false 125 | Name: Current View 126 | Near Clip Distance: 0.00999999978 127 | Pitch: 0.681830764 128 | Target Frame: 129 | Value: Orbit (rviz) 130 | Yaw: 3.78433537 131 | Saved: ~ 132 | Window Geometry: 133 | Displays: 134 | collapsed: false 135 | Height: 1416 136 | Hide Left Dock: false 137 | Hide Right Dock: false 138 | QMainWindow State: 000000ff00000000fd000000040000000000000193000004f6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000004f6000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000121000003f7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003f7000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bf00000046fc0100000002fb0000000800540069006d00650100000000000009bf0000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000826000004f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 139 | Selection: 140 | collapsed: false 141 | Time: 142 | collapsed: false 143 | Tool Properties: 144 | collapsed: false 145 | Views: 146 | collapsed: false 147 | Width: 2495 148 | X: 65 149 | Y: 24 150 | -------------------------------------------------------------------------------- /src/wp_nav_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* @author Zhang Wanjie */ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | typedef actionlib::SimpleActionClient MoveBaseClient; 48 | 49 | int main(int argc, char** argv) 50 | { 51 | ros::init(argc, argv, "wp_nav_test"); 52 | 53 | ros::NodeHandle nh; 54 | ros::ServiceClient cliGetNum = nh.serviceClient("waterplus/get_num_waypoint"); 55 | ros::ServiceClient cliGetWPIndex = nh.serviceClient("waterplus/get_waypoint_index"); 56 | ros::ServiceClient cliGetWPName = nh.serviceClient("waterplus/get_waypoint_name"); 57 | 58 | /////////////////////////////////////////////////////////////////////////////////// 59 | waterplus_map_tools::GetNumOfWaypoints srvNum; 60 | if (cliGetNum.call(srvNum)) 61 | { 62 | ROS_INFO("Num_wp = %d", (int)srvNum.response.num); 63 | } 64 | else 65 | { 66 | ROS_ERROR("Failed to call service get_num_waypoints"); 67 | } 68 | waterplus_map_tools::GetWaypointByIndex srvI; 69 | for(int i=0;i= nNumOfWaypoints) 134 | { 135 | nWPIndex = 0; 136 | continue; 137 | } 138 | 139 | waterplus_map_tools::GetWaypointByIndex srvI; 140 | srvI.request.index = nWPIndex; 141 | 142 | if (cliGetWPIndex.call(srvI)) 143 | { 144 | std::string name = srvI.response.name; 145 | float x = srvI.response.pose.position.x; 146 | float y = srvI.response.pose.position.y; 147 | ROS_INFO("Get_wp_index: name = %s (%.2f,%.2f)", name.c_str(),x,y); 148 | } 149 | else 150 | { 151 | ROS_ERROR("Failed to call service get_wp_index"); 152 | } 153 | 154 | ROS_INFO("Go to the WayPoint[%d]",nWPIndex); 155 | goal.target_pose.header.frame_id = "map"; 156 | goal.target_pose.header.stamp = ros::Time::now(); 157 | goal.target_pose.pose = srvI.response.pose; 158 | ac.sendGoal(goal); 159 | ac.waitForResult(); 160 | 161 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 162 | { 163 | ROS_INFO("Arrived at WayPoint[%d] !",nWPIndex); 164 | nWPIndex ++; 165 | } 166 | else 167 | ROS_INFO("Failed to get to WayPoint[%d] ...",nWPIndex ); 168 | } 169 | 170 | return 0; 171 | } -------------------------------------------------------------------------------- /meshes/charger.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Blender User 6 | Blender 2.81.16 commit date:2019-11-20, commit time:14:27, hash:26bd5ebd42e3 7 | 8 | 2019-12-05T21:00:53 9 | 2019-12-05T21:00:53 10 | 11 | Z_UP 12 | 13 | 14 | 15 | 16 | 17 | 18 | 39.59775 19 | 1.777778 20 | 0.1 21 | 100 22 | 23 | 24 | 25 | 26 | 27 | 0 28 | 0 29 | 10 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 1000 1000 1000 39 | 1 40 | 0 41 | 0.00111109 42 | 43 | 44 | 45 | 46 | 0 47 | 0 48 | 1 49 | 1 50 | 1 51 | 1 52 | 1 53 | 0 54 | 0 55 | 0 56 | 1000 57 | 29.99998 58 | 75 59 | 0.15 60 | 0 61 | 1 62 | 2 63 | 0.04999995 64 | 30.002 65 | 1 66 | 3 67 | 2880 68 | 3 69 | 1 70 | 1 71 | 0.1 72 | 0.1 73 | 1 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 0.11 0 0.1711774 0.11 0 0.0911774 0.11 0.01999998 0.1711774 0.11 0.228 0.0911774 0.11 0.01999998 0.4211775 0.11 0.228 0.5011774 0.11 0 0.4211775 0.11 0 0.5011774 0 0.228 0.4376775 0 0.228 0.1546774 0 0 0.4376775 0 0 0.1546774 0.2249531 0.228 0.5923549 0.2249531 0 0.5923549 0 0 0.4376775 0.2306189 0.228 0.5841149 0.2306189 0 0.5841149 0.11 0 0.5011774 0.2306189 0.228 0.008240044 0.2306189 0 0.008240044 0.11 0 0.0911774 0.2249531 0.228 0 0.2249531 0 0 0 0 0.1546774 0.2249531 0 0 0.11 0 0.1711774 0 0 0.1546774 0.11 0 0.4211775 0.35 0 0.4211775 0.35 0 0.1711774 0.35 0.01999998 0.4211775 0.35 0.01999998 0.1711774 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 1 0 0 -1 0 0 -0.5665839 0 0.8240041 -0.5665842 0 0.8240039 0.8240054 0 0.566582 0.8240012 0 0.5665882 0.5665845 0 -0.8240037 0.5665839 0 -0.8240041 0.5665841 0 0.8240041 0.8240042 0 -0.5665838 -0.5665841 0 -0.824004 0 1 0 0 -1 0 0 0 1 0 0 -1 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 |

0 0 1 0 2 0 2 0 1 0 3 0 2 0 3 0 4 0 4 0 3 0 5 0 4 0 5 0 6 0 6 0 5 0 7 0 8 1 9 1 10 1 10 1 9 1 11 1 12 2 8 2 13 2 13 3 8 3 14 3 15 4 12 4 16 4 16 5 12 5 13 5 5 6 15 6 17 6 17 7 15 7 16 7 18 8 3 8 19 8 19 8 3 8 20 8 21 9 18 9 22 9 22 9 18 9 19 9 9 10 21 10 23 10 23 10 21 10 24 10 12 11 15 11 5 11 18 11 21 11 3 11 3 11 21 11 9 11 3 11 9 11 5 11 5 11 9 11 8 11 5 11 8 11 12 11 22 12 19 12 20 12 25 12 14 12 20 12 20 12 14 12 26 12 20 12 26 12 22 12 16 12 13 12 17 12 17 12 13 12 14 12 17 12 14 12 27 12 27 12 14 12 25 12 27 12 25 12 28 12 28 12 25 12 29 12 28 13 30 13 27 13 27 13 30 13 4 13 31 14 29 14 2 14 2 14 29 14 25 14 30 11 31 11 4 11 4 11 31 11 2 11 29 0 31 0 28 0 28 0 31 0 30 0

109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 1 0 0 -0.11 0 -4.37114e-8 -1 0.295 0 1 -4.37114e-8 0 0 0 0 1 117 | 118 | 119 | 120 | 0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1 121 | 122 | 123 | 124 | -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /src/wp_nav_remote.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* @author Zhang Wanjie */ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | extern "C" { 49 | #include "UDPServer.h" 50 | } 51 | 52 | static tf::StampedTransform transform; 53 | static ST_Ctrl st_ctrl; 54 | 55 | typedef actionlib::SimpleActionClient MoveBaseClient; 56 | 57 | static ros::Publisher behaviors_pub; 58 | static std_msgs::String behavior_msg; 59 | 60 | void GrabResultCB(const std_msgs::String::ConstPtr &msg) 61 | { 62 | ROS_WARN("[GrabResultCB] %s",msg->data.c_str()); 63 | } 64 | 65 | void PassResultCB(const std_msgs::String::ConstPtr &msg) 66 | { 67 | ROS_WARN("[PassResultCB] %s",msg->data.c_str()); 68 | } 69 | 70 | int main(int argc, char** argv) 71 | { 72 | ros::init(argc, argv, "wp_nav_remote"); 73 | 74 | InitUDPServer(20181); //本地监听端口 75 | 76 | ros::NodeHandle nh; 77 | ros::ServiceClient cliGetNum = nh.serviceClient("waterplus/get_num_waypoint"); 78 | ros::ServiceClient cliGetWPIndex = nh.serviceClient("waterplus/get_waypoint_index"); 79 | ros::ServiceClient cliGetWPName = nh.serviceClient("waterplus/get_waypoint_name"); 80 | behaviors_pub = nh.advertise("wpr1/behaviors", 30); 81 | ros::Subscriber res_grab = nh.subscribe("wpr1/grab_result", 30, GrabResultCB); 82 | ros::Subscriber res_pass = nh.subscribe("wpr1/pass_result", 30, PassResultCB); 83 | 84 | /////////////////////////////////////////////////////////////////////////////////// 85 | //把航点名称都列出来 86 | waterplus_map_tools::GetNumOfWaypoints srvNum; 87 | if (cliGetNum.call(srvNum)) 88 | { 89 | ROS_INFO("Num_wp = %d", (int)srvNum.response.num); 90 | } 91 | else 92 | { 93 | ROS_ERROR("Failed to call service get_num_waypoints"); 94 | } 95 | waterplus_map_tools::GetWaypointByIndex srvI; 96 | for(int i=0;i 68 | Value: true 69 | - Alpha: 1 70 | Autocompute Intensity Bounds: true 71 | Autocompute Value Bounds: 72 | Max Value: 10 73 | Min Value: -10 74 | Value: true 75 | Axis: Z 76 | Channel Name: intensity 77 | Class: rviz/LaserScan 78 | Color: 255; 0; 0 79 | Color Transformer: FlatColor 80 | Decay Time: 0 81 | Enabled: true 82 | Invert Rainbow: false 83 | Max Color: 255; 255; 255 84 | Max Intensity: 47 85 | Min Color: 0; 0; 0 86 | Min Intensity: 47 87 | Name: LaserScan 88 | Position Transformer: XYZ 89 | Queue Size: 10 90 | Selectable: true 91 | Size (Pixels): 3 92 | Size (m): 0.03 93 | Style: Flat Squares 94 | Topic: /scan 95 | Unreliable: false 96 | Use Fixed Frame: true 97 | Use rainbow: true 98 | Value: true 99 | - Alpha: 0.7 100 | Class: rviz/Map 101 | Color Scheme: map 102 | Draw Behind: false 103 | Enabled: true 104 | Name: Map 105 | Topic: /map 106 | Unreliable: false 107 | Value: true 108 | - Alpha: 1 109 | Class: rviz/RobotModel 110 | Collision Enabled: false 111 | Enabled: true 112 | Links: 113 | All Links Enabled: true 114 | Expand Joint Details: false 115 | Expand Link Details: false 116 | Expand Tree: false 117 | Link Tree Style: Links in Alphabetic Order 118 | base_footprint: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | base_link: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | body_link: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | kinect2_dock: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Value: true 138 | kinect2_ir_optical_frame: 139 | Alpha: 1 140 | Show Axes: false 141 | Show Trail: false 142 | Value: true 143 | kinect2_rgb_optical_frame: 144 | Alpha: 1 145 | Show Axes: false 146 | Show Trail: false 147 | Value: true 148 | laser: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | Value: true 153 | mani_elbow: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | Value: true 158 | mani_forearm: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | Value: true 163 | mani_left_finger: 164 | Alpha: 1 165 | Show Axes: false 166 | Show Trail: false 167 | Value: true 168 | mani_lift: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | Value: true 173 | mani_right_finger: 174 | Alpha: 1 175 | Show Axes: false 176 | Show Trail: false 177 | Value: true 178 | Name: RobotModel 179 | Robot Description: robot_description 180 | TF Prefix: "" 181 | Update Interval: 0 182 | Value: true 183 | Visual Enabled: true 184 | - Class: rviz/TF 185 | Enabled: false 186 | Frame Timeout: 500 187 | Frames: 188 | All Enabled: true 189 | Marker Scale: 0.5 190 | Name: TF 191 | Show Arrows: true 192 | Show Axes: true 193 | Show Names: true 194 | Tree: 195 | {} 196 | Update Interval: 0 197 | Value: false 198 | - Angle Tolerance: 0.1 199 | Class: rviz/Odometry 200 | Color: 0; 0; 255 201 | Enabled: false 202 | Keep: 50 203 | Length: 0.5 204 | Name: Odometry 205 | Position Tolerance: 0.5 206 | Topic: /odom 207 | Value: false 208 | - Alpha: 1 209 | Buffer Length: 1 210 | Class: rviz/Path 211 | Color: 255; 85; 255 212 | Enabled: true 213 | Head Diameter: 0.3 214 | Head Length: 0.2 215 | Length: 0.3 216 | Line Style: Billboards 217 | Line Width: 0.03 218 | Name: Path 219 | Offset: 220 | X: 0 221 | Y: 0 222 | Z: 0 223 | Pose Color: 255; 85; 255 224 | Pose Style: None 225 | Radius: 0.03 226 | Shaft Diameter: 0.1 227 | Shaft Length: 0.1 228 | Topic: /move_base/GlobalPlanner/plan 229 | Unreliable: false 230 | Value: true 231 | - Alpha: 0.7 232 | Class: rviz/Map 233 | Color Scheme: costmap 234 | Draw Behind: false 235 | Enabled: true 236 | Name: Local Costmap 237 | Topic: /move_base/local_costmap/costmap 238 | Unreliable: false 239 | Value: true 240 | - Alpha: 0.7 241 | Class: rviz/Map 242 | Color Scheme: costmap 243 | Draw Behind: false 244 | Enabled: true 245 | Name: Global Costmap 246 | Topic: /move_base/global_costmap/costmap 247 | Unreliable: false 248 | Value: true 249 | - Arrow Length: 0.3 250 | Class: rviz/PoseArray 251 | Color: 0; 255; 0 252 | Enabled: true 253 | Name: Particle Cloud 254 | Topic: /particlecloud 255 | Unreliable: false 256 | Value: true 257 | - Alpha: 1 258 | Class: rviz/Polygon 259 | Color: 255; 0; 255 260 | Enabled: true 261 | Name: footprint 262 | Topic: /move_base/global_costmap/footprint 263 | Unreliable: false 264 | Value: true 265 | - Alpha: 1 266 | Autocompute Intensity Bounds: true 267 | Autocompute Value Bounds: 268 | Max Value: 10 269 | Min Value: -10 270 | Value: true 271 | Axis: Z 272 | Channel Name: intensity 273 | Class: rviz/PointCloud2 274 | Color: 255; 255; 255 275 | Color Transformer: RGB8 276 | Decay Time: 0 277 | Enabled: true 278 | Invert Rainbow: false 279 | Max Color: 255; 255; 255 280 | Max Intensity: 4096 281 | Min Color: 0; 0; 0 282 | Min Intensity: 0 283 | Name: Kinect 284 | Position Transformer: XYZ 285 | Queue Size: 10 286 | Selectable: true 287 | Size (Pixels): 3 288 | Size (m): 0.02 289 | Style: Flat Squares 290 | Topic: /kinect2/qhd/points 291 | Unreliable: false 292 | Use Fixed Frame: true 293 | Use rainbow: true 294 | Value: true 295 | - Class: rviz/Marker 296 | Enabled: true 297 | Marker Topic: /waypoints_marker 298 | Name: Marker_Waypoint 299 | Namespaces: 300 | marker_waypoints: true 301 | text: true 302 | Queue Size: 100 303 | Value: true 304 | - Class: rviz/Marker 305 | Enabled: true 306 | Marker Topic: visualization_marker 307 | Name: Text 308 | Namespaces: 309 | {} 310 | Queue Size: 100 311 | Value: true 312 | - Alpha: 1 313 | Autocompute Intensity Bounds: true 314 | Autocompute Value Bounds: 315 | Max Value: 10 316 | Min Value: -10 317 | Value: true 318 | Axis: Z 319 | Channel Name: intensity 320 | Class: rviz/PointCloud2 321 | Color: 255; 255; 255 322 | Color Transformer: RGB8 323 | Decay Time: 0 324 | Enabled: true 325 | Invert Rainbow: false 326 | Max Color: 255; 255; 255 327 | Max Intensity: 4096 328 | Min Color: 0; 0; 0 329 | Min Intensity: 0 330 | Name: PointCloud2 331 | Position Transformer: XYZ 332 | Queue Size: 10 333 | Selectable: true 334 | Size (Pixels): 3 335 | Size (m): 0.01 336 | Style: Flat Squares 337 | Topic: /kinect2/sd/points 338 | Unreliable: false 339 | Use Fixed Frame: true 340 | Use rainbow: true 341 | Value: true 342 | - Class: rviz/Marker 343 | Enabled: true 344 | Marker Topic: /obj_marker 345 | Name: Marker_Obj 346 | Namespaces: 347 | line_box: true 348 | line_obj: true 349 | Queue Size: 100 350 | Value: true 351 | Enabled: true 352 | Global Options: 353 | Background Color: 48; 48; 48 354 | Fixed Frame: map 355 | Frame Rate: 30 356 | Name: root 357 | Tools: 358 | - Class: rviz/Interact 359 | Hide Inactive Objects: true 360 | - Class: rviz/MoveCamera 361 | - Class: rviz/Select 362 | - Class: rviz/FocusCamera 363 | - Class: rviz/Measure 364 | - Class: rviz/SetInitialPose 365 | Topic: /initialpose 366 | - Class: rviz/SetGoal 367 | Topic: /move_base_simple/goal 368 | - Class: rviz/PublishPoint 369 | Single click: true 370 | Topic: /clicked_point 371 | - Class: rviz/AddWaypoint 372 | Topic: /waterplus/add_waypoint 373 | Value: true 374 | Views: 375 | Current: 376 | Class: rviz/Orbit 377 | Distance: 3.28069 378 | Enable Stereo Rendering: 379 | Stereo Eye Separation: 0.06 380 | Stereo Focal Distance: 1 381 | Swap Stereo Eyes: false 382 | Value: false 383 | Focal Point: 384 | X: -0.892056 385 | Y: 0.238437 386 | Z: 1.46426 387 | Name: Current View 388 | Near Clip Distance: 0.01 389 | Pitch: 0.84683 390 | Target Frame: 391 | Value: Orbit (rviz) 392 | Yaw: 3.14432 393 | Saved: ~ 394 | Window Geometry: 395 | Displays: 396 | collapsed: false 397 | Height: 1056 398 | Hide Left Dock: false 399 | Hide Right Dock: false 400 | QMainWindow State: 000000ff00000000fd00000004000000000000018e00000389fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006800fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000389000000e600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000121000003f7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003f7000000be00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000072f00000046fc0100000002fb0000000800540069006d006501000000000000072f0000038800fffffffb0000000800540069006d006501000000000000045000000000000000000000059b0000038900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 401 | Selection: 402 | collapsed: false 403 | Time: 404 | collapsed: false 405 | Tool Properties: 406 | collapsed: false 407 | Views: 408 | collapsed: false 409 | Width: 1839 410 | X: 70 411 | Y: 18 412 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | 294 | Copyright (C) 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | , 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | -------------------------------------------------------------------------------- /src/wp_manager.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* @author Zhang Wanjie */ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | static std::vector arWaypoint; 48 | static std::vector arCharger; 49 | static ros::Publisher marker_pub; 50 | static ros::Publisher charger_pub; 51 | static visualization_msgs::Marker marker_waypoints; 52 | static visualization_msgs::Marker marker_chargers; 53 | static visualization_msgs::Marker text_marker; 54 | 55 | bool getNumOfWaypoints(waterplus_map_tools::GetNumOfWaypoints::Request &req, waterplus_map_tools::GetNumOfWaypoints::Response &res) 56 | { 57 | res.num = arWaypoint.size(); 58 | ROS_INFO("Get_num_wp: num_wp = %d", res.num); 59 | return true; 60 | } 61 | 62 | bool getWaypointByIndex(waterplus_map_tools::GetWaypointByIndex::Request &req, waterplus_map_tools::GetWaypointByIndex::Response &res) 63 | { 64 | int nIndex = req.index; 65 | int nNumWP = arWaypoint.size(); 66 | if(nIndex >= 0 && nIndex < nNumWP) 67 | { 68 | res.name = arWaypoint[nIndex].name; 69 | res.pose = arWaypoint[nIndex].pose; 70 | ROS_INFO("Get_wp_index: name = %s", arWaypoint[nIndex].name.c_str()); 71 | return true; 72 | } 73 | else 74 | { 75 | ROS_INFO("Get_wp_index: failed! index = %d , num_wp = %d", nIndex , nNumWP); 76 | return false; 77 | } 78 | } 79 | 80 | bool getWaypointByName(waterplus_map_tools::GetWaypointByName::Request &req, waterplus_map_tools::GetWaypointByName::Response &res) 81 | { 82 | std::string reqName = req.name; 83 | int nNumWP = arWaypoint.size(); 84 | bool bResultGetWP = false; 85 | for(int i=0;i= 0 && nIndex < nNumCh) 120 | { 121 | res.name = arCharger[nIndex].name; 122 | res.pose = arCharger[nIndex].pose; 123 | ROS_INFO("Get_ch_index: name = %s", arCharger[nIndex].name.c_str()); 124 | return true; 125 | } 126 | else 127 | { 128 | ROS_INFO("Get_ch_index: failed! index = %d , num_ch= %d", nIndex , nNumCh); 129 | return false; 130 | } 131 | } 132 | 133 | bool getChargerByName(waterplus_map_tools::GetWaypointByName::Request &req, waterplus_map_tools::GetWaypointByName::Response &res) 134 | { 135 | std::string reqName = req.name; 136 | int nNumCh = arCharger.size(); 137 | bool bResultGetCh = false; 138 | for(int i=0;iLinkEndChild(RootElement); 188 | 189 | int nNumWP = arWaypoint.size(); 190 | for(int i=0;iInsertEndChild(TiXmlElement("Name"))->InsertEndChild(TiXmlText(arWaypoint[i].name)); 194 | WaypointElement->InsertEndChild(TiXmlElement("Pos_x"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.x))); 195 | WaypointElement->InsertEndChild(TiXmlElement("Pos_y"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.y))); 196 | WaypointElement->InsertEndChild(TiXmlElement("Pos_z"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.z))); 197 | WaypointElement->InsertEndChild(TiXmlElement("Ori_x"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.x))); 198 | WaypointElement->InsertEndChild(TiXmlElement("Ori_y"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.y))); 199 | WaypointElement->InsertEndChild(TiXmlElement("Ori_z"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.z))); 200 | WaypointElement->InsertEndChild(TiXmlElement("Ori_w"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.w))); 201 | RootElement->InsertEndChild(*WaypointElement); 202 | } 203 | 204 | int nNumCharger = arCharger.size(); 205 | for(int i=0;iInsertEndChild(TiXmlElement("Name"))->InsertEndChild(TiXmlText(arCharger[i].name)); 209 | ChargerElement->InsertEndChild(TiXmlElement("Pos_x"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.x))); 210 | ChargerElement->InsertEndChild(TiXmlElement("Pos_y"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.y))); 211 | ChargerElement->InsertEndChild(TiXmlElement("Pos_z"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.z))); 212 | ChargerElement->InsertEndChild(TiXmlElement("Ori_x"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.x))); 213 | ChargerElement->InsertEndChild(TiXmlElement("Ori_y"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.y))); 214 | ChargerElement->InsertEndChild(TiXmlElement("Ori_z"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.z))); 215 | ChargerElement->InsertEndChild(TiXmlElement("Ori_w"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.w))); 216 | RootElement->InsertEndChild(*ChargerElement); 217 | } 218 | 219 | bool res = docSave->SaveFile(inFilename); 220 | if(res == true) 221 | ROS_INFO("Saved waypoints to file! filename = %s", inFilename.c_str()); 222 | else 223 | ROS_INFO("Failed to save waypoints... filename = %s", inFilename.c_str()); 224 | 225 | return res; 226 | } 227 | 228 | bool LoadWaypointsFromFile(std::string inFilename) 229 | { 230 | TiXmlDocument docLoad(inFilename); 231 | bool resLoad = docLoad.LoadFile(); 232 | if(resLoad == false) 233 | { 234 | ROS_INFO("Failed to load waypoints... filename = %s", inFilename.c_str()); 235 | return false; 236 | } 237 | 238 | waterplus_map_tools::Waypoint newWayPoint; 239 | TiXmlElement* RootElement = docLoad.RootElement(); 240 | for(TiXmlNode* item = RootElement->FirstChild("Waypoint");item;item = item->NextSibling("Waypoint")) 241 | { 242 | TiXmlNode* child = item->FirstChild(); 243 | const char* name = child->ToElement()->GetText(); 244 | ROS_INFO("Load waypoint : %s", name); 245 | newWayPoint.name = std::string(name); 246 | child = item->IterateChildren(child); 247 | const char* pos_x = child->ToElement()->GetText(); 248 | newWayPoint.pose.position.x = std::atof(pos_x); 249 | child = item->IterateChildren(child); 250 | const char* pos_y = child->ToElement()->GetText(); 251 | newWayPoint.pose.position.y = std::atof(pos_y); 252 | child = item->IterateChildren(child); 253 | const char* pos_z = child->ToElement()->GetText(); 254 | newWayPoint.pose.position.z = std::atof(pos_z); 255 | child = item->IterateChildren(child); 256 | const char* ori_x = child->ToElement()->GetText(); 257 | newWayPoint.pose.orientation.x = std::atof(ori_x); 258 | child = item->IterateChildren(child); 259 | const char* ori_y = child->ToElement()->GetText(); 260 | newWayPoint.pose.orientation.y = std::atof(ori_y); 261 | child = item->IterateChildren(child); 262 | const char* ori_z = child->ToElement()->GetText(); 263 | newWayPoint.pose.orientation.z = std::atof(ori_z); 264 | child = item->IterateChildren(child); 265 | const char* ori_w = child->ToElement()->GetText(); 266 | newWayPoint.pose.orientation.w = std::atof(ori_w); 267 | arWaypoint.push_back(newWayPoint); 268 | } 269 | 270 | for(TiXmlNode* item = RootElement->FirstChild("Charger");item;item = item->NextSibling("Charger")) 271 | { 272 | TiXmlNode* child = item->FirstChild(); 273 | const char* name = child->ToElement()->GetText(); 274 | ROS_INFO("Load charger : %s", name); 275 | newWayPoint.name = std::string(name); 276 | child = item->IterateChildren(child); 277 | const char* pos_x = child->ToElement()->GetText(); 278 | newWayPoint.pose.position.x = std::atof(pos_x); 279 | child = item->IterateChildren(child); 280 | const char* pos_y = child->ToElement()->GetText(); 281 | newWayPoint.pose.position.y = std::atof(pos_y); 282 | child = item->IterateChildren(child); 283 | const char* pos_z = child->ToElement()->GetText(); 284 | newWayPoint.pose.position.z = std::atof(pos_z); 285 | child = item->IterateChildren(child); 286 | const char* ori_x = child->ToElement()->GetText(); 287 | newWayPoint.pose.orientation.x = std::atof(ori_x); 288 | child = item->IterateChildren(child); 289 | const char* ori_y = child->ToElement()->GetText(); 290 | newWayPoint.pose.orientation.y = std::atof(ori_y); 291 | child = item->IterateChildren(child); 292 | const char* ori_z = child->ToElement()->GetText(); 293 | newWayPoint.pose.orientation.z = std::atof(ori_z); 294 | child = item->IterateChildren(child); 295 | const char* ori_w = child->ToElement()->GetText(); 296 | newWayPoint.pose.orientation.w = std::atof(ori_w); 297 | arCharger.push_back(newWayPoint); 298 | } 299 | 300 | return true; 301 | } 302 | 303 | void Init_Marker() 304 | { 305 | marker_waypoints.header.frame_id = "map"; 306 | marker_waypoints.ns = "marker_waypoints"; 307 | marker_waypoints.action = visualization_msgs::Marker::ADD; 308 | marker_waypoints.type = visualization_msgs::Marker::MESH_RESOURCE; 309 | marker_waypoints.mesh_resource = "package://waterplus_map_tools/meshes/waypoint.dae"; 310 | marker_waypoints.scale.x = 1; 311 | marker_waypoints.scale.y = 1; 312 | marker_waypoints.scale.z = 1; 313 | marker_waypoints.color.r = 1.0; 314 | marker_waypoints.color.g = 0.0; 315 | marker_waypoints.color.b = 1.0; 316 | marker_waypoints.color.a = 1.0; 317 | 318 | marker_chargers.header.frame_id = "map"; 319 | marker_chargers.ns = "marker_waypoints"; 320 | marker_chargers.action = visualization_msgs::Marker::ADD; 321 | marker_chargers.type = visualization_msgs::Marker::MESH_RESOURCE; 322 | marker_chargers.mesh_resource = "package://waterplus_map_tools/meshes/charger.dae"; 323 | marker_chargers.scale.x = 1; 324 | marker_chargers.scale.y = 1; 325 | marker_chargers.scale.z = 1; 326 | marker_chargers.color.r = 0.5; 327 | marker_chargers.color.g = 0.0; 328 | marker_chargers.color.b = 1.0; 329 | marker_chargers.color.a = 1.0; 330 | } 331 | 332 | void DrawTextMarker(ros::Publisher* inPub, std::string inText, int inID, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB); 333 | void PublishWaypointsMarker() 334 | { 335 | int nNumWP = arWaypoint.size(); 336 | for(int i=0; ipublish(text_marker); 397 | } 398 | 399 | void AddWayPointCallback(const waterplus_map_tools::Waypoint::ConstPtr& wp) 400 | { 401 | ROS_INFO("Add_waypoint: %s (%.2f %.2f) (%.2f %.2f %.2f %.2f) ",wp->name.c_str(), wp->pose.position.x, wp->pose.position.y, wp->pose.orientation.x, wp->pose.orientation.y, wp->pose.orientation.z, wp->pose.orientation.w); 402 | waterplus_map_tools::Waypoint newWayPoint; 403 | newWayPoint = *wp; 404 | arWaypoint.push_back(newWayPoint); 405 | } 406 | 407 | void AddChargerCallback(const waterplus_map_tools::Waypoint::ConstPtr& wp) 408 | { 409 | ROS_INFO("Add_charger: %s (%.2f %.2f) (%.2f %.2f %.2f %.2f) ",wp->name.c_str(), wp->pose.position.x, wp->pose.position.y, wp->pose.orientation.x, wp->pose.orientation.y, wp->pose.orientation.z, wp->pose.orientation.w); 410 | waterplus_map_tools::Waypoint newCharger; 411 | newCharger = *wp; 412 | arCharger.push_back(newCharger); 413 | } 414 | 415 | int main(int argc, char** argv) 416 | { 417 | ros::init(argc, argv, "wp_waypoint_manager"); 418 | 419 | std::string strLoadFile; 420 | char const* home = getenv("HOME"); 421 | strLoadFile = home; 422 | strLoadFile += "/waypoints.xml"; 423 | 424 | ros::NodeHandle n_param("~"); 425 | std::string strParamFile; 426 | n_param.param("load", strParamFile, ""); 427 | if(strParamFile.length() > 0) 428 | { 429 | strLoadFile = strParamFile; 430 | } 431 | 432 | if(strLoadFile.length() > 0) 433 | { 434 | ROS_INFO("Load waypoints from file : %s",strLoadFile.c_str()); 435 | LoadWaypointsFromFile(strLoadFile); 436 | } 437 | else 438 | { 439 | ROS_WARN("strLoadFile is empty. Failed to load waypoints!"); 440 | } 441 | 442 | 443 | ros::NodeHandle nh; 444 | marker_pub = nh.advertise("waypoints_marker", 100); 445 | charger_pub = nh.advertise("chargers_marker", 100); 446 | Init_Marker(); 447 | ros::Subscriber add_waypoint_sub = nh.subscribe("waterplus/add_waypoint",10,&AddWayPointCallback); 448 | ros::Subscriber add_charger_sub = nh.subscribe("waterplus/add_charger",10,&AddChargerCallback); 449 | 450 | ros::ServiceServer srvGetNum = nh.advertiseService("waterplus/get_num_waypoint", getNumOfWaypoints); 451 | ros::ServiceServer srvGetWPIndex = nh.advertiseService("waterplus/get_waypoint_index", getWaypointByIndex); 452 | ros::ServiceServer srvGetWPName = nh.advertiseService("waterplus/get_waypoint_name", getWaypointByName); 453 | ros::ServiceServer srvSaveWP = nh.advertiseService("waterplus/save_waypoints", saveWaypoints); 454 | 455 | ros::ServiceServer srvGetChargerNum = nh.advertiseService("waterplus/get_num_charger", getNumOfChargers); 456 | ros::ServiceServer srvGetChargerIndex = nh.advertiseService("waterplus/get_charger_index", getChargerByIndex); 457 | ros::ServiceServer srvGetChargerName = nh.advertiseService("waterplus/get_charger_name", getChargerByName); 458 | 459 | ros::Rate r(10); 460 | 461 | while(ros::ok()) 462 | { 463 | PublishWaypointsMarker(); 464 | ros::spinOnce(); 465 | r.sleep(); 466 | } 467 | 468 | return 0; 469 | } -------------------------------------------------------------------------------- /src/wp_edit_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2017-2020, Waterplus http://www.6-robot.com 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the WaterPlus nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* @author Zhang Wanjie */ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | using namespace visualization_msgs; 50 | using namespace interactive_markers; 51 | using namespace std; 52 | 53 | static std::vector arWaypoint; 54 | static std::vector arCharger; 55 | static ros::Publisher marker_pub; 56 | static visualization_msgs::Marker text_marker; 57 | static InteractiveMarkerServer* pWaypointServer = NULL; 58 | static MenuHandler* pMenuWaypoint = NULL; 59 | static MenuHandler* pMenuCharger = NULL; 60 | 61 | bool bDeleteWaypoint = false; 62 | std::string strDelWaypointName; 63 | bool bDeleteCharger = false; 64 | std::string strDelChargerName; 65 | 66 | bool getNumOfWaypoints(waterplus_map_tools::GetNumOfWaypoints::Request &req, waterplus_map_tools::GetNumOfWaypoints::Response &res) 67 | { 68 | res.num = arWaypoint.size(); 69 | ROS_INFO("Get_num_wp: num_wp = %d", res.num); 70 | return true; 71 | } 72 | 73 | bool getWaypointByIndex(waterplus_map_tools::GetWaypointByIndex::Request &req, waterplus_map_tools::GetWaypointByIndex::Response &res) 74 | { 75 | int nIndex = req.index; 76 | int nNumWP = arWaypoint.size(); 77 | if(nIndex >= 0 && nIndex < nNumWP) 78 | { 79 | res.name = arWaypoint[nIndex].name; 80 | res.pose = arWaypoint[nIndex].pose; 81 | ROS_INFO("Get_wp_index: name = %s", arWaypoint[nIndex].name.c_str()); 82 | return true; 83 | } 84 | else 85 | { 86 | ROS_INFO("Get_wp_index: failed! index = %d , num_wp = %d", nIndex , nNumWP); 87 | return false; 88 | } 89 | } 90 | 91 | bool getWaypointByName(waterplus_map_tools::GetWaypointByName::Request &req, waterplus_map_tools::GetWaypointByName::Response &res) 92 | { 93 | std::string reqName = req.name; 94 | int nNumWP = arWaypoint.size(); 95 | bool bResultGetWP = false; 96 | for(int i=0;iLinkEndChild(RootElement); 174 | 175 | int nNumWP = arWaypoint.size(); 176 | for(int i=0;iInsertEndChild(TiXmlElement("Name"))->InsertEndChild(TiXmlText(arWaypoint[i].name)); 180 | WaypointElement->InsertEndChild(TiXmlElement("Pos_x"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.x))); 181 | WaypointElement->InsertEndChild(TiXmlElement("Pos_y"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.y))); 182 | WaypointElement->InsertEndChild(TiXmlElement("Pos_z"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.z))); 183 | WaypointElement->InsertEndChild(TiXmlElement("Ori_x"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.x))); 184 | WaypointElement->InsertEndChild(TiXmlElement("Ori_y"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.y))); 185 | WaypointElement->InsertEndChild(TiXmlElement("Ori_z"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.z))); 186 | WaypointElement->InsertEndChild(TiXmlElement("Ori_w"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.w))); 187 | RootElement->InsertEndChild(*WaypointElement); 188 | } 189 | 190 | int nNumCharger = arCharger.size(); 191 | for(int i=0;iInsertEndChild(TiXmlElement("Name"))->InsertEndChild(TiXmlText(arCharger[i].name)); 195 | ChargerElement->InsertEndChild(TiXmlElement("Pos_x"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.x))); 196 | ChargerElement->InsertEndChild(TiXmlElement("Pos_y"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.y))); 197 | ChargerElement->InsertEndChild(TiXmlElement("Pos_z"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.z))); 198 | ChargerElement->InsertEndChild(TiXmlElement("Ori_x"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.x))); 199 | ChargerElement->InsertEndChild(TiXmlElement("Ori_y"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.y))); 200 | ChargerElement->InsertEndChild(TiXmlElement("Ori_z"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.z))); 201 | ChargerElement->InsertEndChild(TiXmlElement("Ori_w"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.w))); 202 | RootElement->InsertEndChild(*ChargerElement); 203 | } 204 | 205 | bool res = docSave->SaveFile(inFilename); 206 | if(res == true) 207 | ROS_INFO("Saved waypoints to file! filename = %s", inFilename.c_str()); 208 | else 209 | ROS_INFO("Failed to save waypoints... filename = %s", inFilename.c_str()); 210 | 211 | return res; 212 | } 213 | 214 | bool LoadWaypointsFromFile(std::string inFilename) 215 | { 216 | TiXmlDocument docLoad(inFilename); 217 | bool resLoad = docLoad.LoadFile(); 218 | if(resLoad == false) 219 | { 220 | ROS_INFO("Failed to load waypoints... filename = %s", inFilename.c_str()); 221 | return false; 222 | } 223 | 224 | waterplus_map_tools::Waypoint newWayPoint; 225 | TiXmlElement* RootElement = docLoad.RootElement(); 226 | for(TiXmlNode* item = RootElement->FirstChild("Waypoint");item;item = item->NextSibling("Waypoint")) 227 | { 228 | TiXmlNode* child = item->FirstChild(); 229 | const char* name = child->ToElement()->GetText(); 230 | ROS_INFO("Load waypoint : %s", name); 231 | newWayPoint.name = std::string(name); 232 | child = item->IterateChildren(child); 233 | const char* pos_x = child->ToElement()->GetText(); 234 | newWayPoint.pose.position.x = std::atof(pos_x); 235 | child = item->IterateChildren(child); 236 | const char* pos_y = child->ToElement()->GetText(); 237 | newWayPoint.pose.position.y = std::atof(pos_y); 238 | child = item->IterateChildren(child); 239 | const char* pos_z = child->ToElement()->GetText(); 240 | newWayPoint.pose.position.z = std::atof(pos_z); 241 | child = item->IterateChildren(child); 242 | const char* ori_x = child->ToElement()->GetText(); 243 | newWayPoint.pose.orientation.x = std::atof(ori_x); 244 | child = item->IterateChildren(child); 245 | const char* ori_y = child->ToElement()->GetText(); 246 | newWayPoint.pose.orientation.y = std::atof(ori_y); 247 | child = item->IterateChildren(child); 248 | const char* ori_z = child->ToElement()->GetText(); 249 | newWayPoint.pose.orientation.z = std::atof(ori_z); 250 | child = item->IterateChildren(child); 251 | const char* ori_w = child->ToElement()->GetText(); 252 | newWayPoint.pose.orientation.w = std::atof(ori_w); 253 | arWaypoint.push_back(newWayPoint); 254 | } 255 | 256 | for(TiXmlNode* item = RootElement->FirstChild("Charger");item;item = item->NextSibling("Charger")) 257 | { 258 | TiXmlNode* child = item->FirstChild(); 259 | const char* name = child->ToElement()->GetText(); 260 | ROS_INFO("Load charger : %s", name); 261 | newWayPoint.name = std::string(name); 262 | child = item->IterateChildren(child); 263 | const char* pos_x = child->ToElement()->GetText(); 264 | newWayPoint.pose.position.x = std::atof(pos_x); 265 | child = item->IterateChildren(child); 266 | const char* pos_y = child->ToElement()->GetText(); 267 | newWayPoint.pose.position.y = std::atof(pos_y); 268 | child = item->IterateChildren(child); 269 | const char* pos_z = child->ToElement()->GetText(); 270 | newWayPoint.pose.position.z = std::atof(pos_z); 271 | child = item->IterateChildren(child); 272 | const char* ori_x = child->ToElement()->GetText(); 273 | newWayPoint.pose.orientation.x = std::atof(ori_x); 274 | child = item->IterateChildren(child); 275 | const char* ori_y = child->ToElement()->GetText(); 276 | newWayPoint.pose.orientation.y = std::atof(ori_y); 277 | child = item->IterateChildren(child); 278 | const char* ori_z = child->ToElement()->GetText(); 279 | newWayPoint.pose.orientation.z = std::atof(ori_z); 280 | child = item->IterateChildren(child); 281 | const char* ori_w = child->ToElement()->GetText(); 282 | newWayPoint.pose.orientation.w = std::atof(ori_w); 283 | arCharger.push_back(newWayPoint); 284 | } 285 | 286 | return true; 287 | } 288 | 289 | void DrawTextMarker(ros::Publisher* inPub, std::string inText, int inID, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB) 290 | { 291 | text_marker.header.frame_id = "map"; 292 | text_marker.ns = "text"; 293 | text_marker.action = visualization_msgs::Marker::ADD; 294 | text_marker.id = inID; 295 | text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 296 | text_marker.scale.z = inScale; 297 | text_marker.color.r = inR; 298 | text_marker.color.g = inG; 299 | text_marker.color.b = inB; 300 | text_marker.color.a = 1.0; 301 | 302 | text_marker.pose.position.x = inX; 303 | text_marker.pose.position.y = inY; 304 | text_marker.pose.position.z = inZ; 305 | 306 | text_marker.pose.orientation=tf::createQuaternionMsgFromYaw(1.0); 307 | 308 | text_marker.text = inText; 309 | 310 | inPub->publish(text_marker); 311 | } 312 | 313 | void RemoveTextMarker() 314 | { 315 | text_marker.action = 3; 316 | marker_pub.publish(text_marker); 317 | } 318 | 319 | void PublishTextMarker() 320 | { 321 | int nNumWP = arWaypoint.size(); 322 | for(int i=0; imarker_name.c_str(), 349 | feedback->pose.position.x,feedback->pose.position.y,feedback->pose.position.z, 350 | feedback->pose.orientation.x,feedback->pose.orientation.y,feedback->pose.orientation.z,feedback->pose.orientation.w); 351 | 352 | int nNumWP = arWaypoint.size(); 353 | for(int i=0; imarker_name == arWaypoint[i].name) 356 | { 357 | arWaypoint[i].pose = feedback->pose; 358 | } 359 | } 360 | 361 | std::ostringstream s; 362 | std::ostringstream mouse_point_ss; 363 | if( feedback->mouse_point_valid ) 364 | { 365 | switch(feedback->event_type) 366 | { 367 | case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: 368 | //ROS_INFO("BUTTON_CLICK"); 369 | break; 370 | case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN: 371 | //ROS_INFO("MOUSE_DOWN"); 372 | break; 373 | case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP: 374 | //ROS_INFO("MOUSE_UP"); 375 | break; 376 | case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT: 377 | //ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." ); 378 | break; 379 | } 380 | } 381 | } 382 | 383 | // 移动充电桩的回调函数 384 | void processChargerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 385 | { 386 | ROS_WARN("[%s] p(%.2f,%.2f,%.2f) r(%.2f,%.2f,%.2f,%.2f)",feedback->marker_name.c_str(), 387 | feedback->pose.position.x,feedback->pose.position.y,feedback->pose.position.z, 388 | feedback->pose.orientation.x,feedback->pose.orientation.y,feedback->pose.orientation.z,feedback->pose.orientation.w); 389 | 390 | int nNumChargers = arCharger.size(); 391 | for(int i=0; imarker_name == arCharger[i].name) 394 | { 395 | arCharger[i].pose = feedback->pose; 396 | } 397 | } 398 | 399 | std::ostringstream s; 400 | std::ostringstream mouse_point_ss; 401 | if( feedback->mouse_point_valid ) 402 | { 403 | switch(feedback->event_type) 404 | { 405 | case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: 406 | //ROS_INFO("BUTTON_CLICK"); 407 | break; 408 | case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN: 409 | //ROS_INFO("MOUSE_DOWN"); 410 | break; 411 | case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP: 412 | //ROS_INFO("MOUSE_UP"); 413 | break; 414 | case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT: 415 | //ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." ); 416 | break; 417 | } 418 | } 419 | } 420 | 421 | // 向服务器添加新的航点操作标记 422 | void NewWaypointInterMarker(InteractiveMarkerServer* inServer,string inName, geometry_msgs::Pose InPose) 423 | { 424 | visualization_msgs::InteractiveMarker wp_itr_marker; 425 | visualization_msgs::InteractiveMarkerControl wp_dis_ctrl; 426 | visualization_msgs::Marker wp_dis_marker; 427 | visualization_msgs::InteractiveMarkerControl move_control; 428 | wp_itr_marker.header.stamp=ros::Time::now(); 429 | wp_itr_marker.name = inName; 430 | wp_itr_marker.description = inName; 431 | wp_itr_marker.pose = InPose; 432 | 433 | // 显示外形 434 | wp_itr_marker.header.frame_id = "map"; 435 | wp_itr_marker.header.stamp=ros::Time::now(); 436 | 437 | wp_dis_marker.ns = "marker_waypoints"; 438 | wp_dis_marker.action = visualization_msgs::Marker::ADD; 439 | wp_dis_marker.type = visualization_msgs::Marker::MESH_RESOURCE; 440 | wp_dis_marker.mesh_resource = "package://waterplus_map_tools/meshes/waypoint.dae"; 441 | wp_dis_marker.scale.x = 1; 442 | wp_dis_marker.scale.y = 1; 443 | wp_dis_marker.scale.z = 1; 444 | wp_dis_marker.color.r = 1.0; 445 | wp_dis_marker.color.g = 0.0; 446 | wp_dis_marker.color.b = 1.0; 447 | wp_dis_marker.color.a = 1.0; 448 | 449 | wp_dis_ctrl.markers.push_back( wp_dis_marker ); 450 | wp_dis_ctrl.always_visible = true; 451 | wp_itr_marker.controls.push_back( wp_dis_ctrl ); 452 | 453 | // 操作设置 454 | move_control.name = "move_x"; 455 | move_control.orientation.w = 1.0; 456 | move_control.orientation.x = 1.0; 457 | move_control.orientation.y = 0.0; 458 | move_control.orientation.z = 0.0; 459 | move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 460 | wp_itr_marker.controls.push_back(move_control); 461 | move_control.name = "move_z"; 462 | move_control.orientation.x = 0.0; 463 | move_control.orientation.z = 1.0; 464 | wp_itr_marker.controls.push_back(move_control); 465 | move_control.name = "rotate_z"; 466 | move_control.orientation.x = 0.0; 467 | move_control.orientation.y = 1.0; 468 | move_control.orientation.z = 0.0; 469 | move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 470 | wp_itr_marker.controls.push_back(move_control); 471 | 472 | // 加入菜单 473 | visualization_msgs::Marker menu_marker; 474 | menu_marker.type = visualization_msgs::Marker::CUBE; 475 | menu_marker.scale.x = 0.5; 476 | menu_marker.scale.y = 0.5; 477 | menu_marker.scale.z = 0.5; 478 | menu_marker.color.r = 0.9; 479 | menu_marker.color.g = 0.9; 480 | menu_marker.color.b = 0.9; 481 | menu_marker.color.a = 0.0; //全透明 482 | InteractiveMarkerControl menu_control; 483 | menu_control.interaction_mode = InteractiveMarkerControl::BUTTON; 484 | menu_control.always_visible = true; 485 | menu_control.markers.push_back( menu_marker ); 486 | wp_itr_marker.controls.push_back( menu_control ); 487 | 488 | inServer->insert(wp_itr_marker, &processWaypointFeedback); 489 | } 490 | 491 | // 向服务器添加新的充电桩操作标记 492 | void NewChargerInterMarker(InteractiveMarkerServer* inServer,string inName, geometry_msgs::Pose InPose) 493 | { 494 | visualization_msgs::InteractiveMarker wp_itr_marker; 495 | visualization_msgs::InteractiveMarkerControl wp_dis_ctrl; 496 | visualization_msgs::Marker wp_dis_marker; 497 | visualization_msgs::InteractiveMarkerControl move_control; 498 | wp_itr_marker.header.stamp=ros::Time::now(); 499 | wp_itr_marker.name = inName; 500 | wp_itr_marker.description = inName; 501 | wp_itr_marker.pose = InPose; 502 | 503 | // 显示外形 504 | wp_itr_marker.header.frame_id = "map"; 505 | wp_itr_marker.header.stamp=ros::Time::now(); 506 | 507 | wp_dis_marker.ns = "marker_chargers"; 508 | wp_dis_marker.action = visualization_msgs::Marker::ADD; 509 | wp_dis_marker.type = visualization_msgs::Marker::MESH_RESOURCE; 510 | wp_dis_marker.mesh_resource = "package://waterplus_map_tools/meshes/charger.dae"; 511 | wp_dis_marker.scale.x = 1; 512 | wp_dis_marker.scale.y = 1; 513 | wp_dis_marker.scale.z = 1; 514 | wp_dis_marker.color.r = 0.0; 515 | wp_dis_marker.color.g = 0.0; 516 | wp_dis_marker.color.b = 1.0; 517 | wp_dis_marker.color.a = 1.0; 518 | 519 | wp_dis_ctrl.markers.push_back( wp_dis_marker ); 520 | wp_dis_ctrl.always_visible = true; 521 | wp_itr_marker.controls.push_back( wp_dis_ctrl ); 522 | 523 | // 操作设置 524 | move_control.name = "move_x"; 525 | move_control.orientation.w = 1.0; 526 | move_control.orientation.x = 1.0; 527 | move_control.orientation.y = 0.0; 528 | move_control.orientation.z = 0.0; 529 | move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 530 | wp_itr_marker.controls.push_back(move_control); 531 | move_control.name = "move_z"; 532 | move_control.orientation.x = 0.0; 533 | move_control.orientation.z = 1.0; 534 | wp_itr_marker.controls.push_back(move_control); 535 | move_control.name = "rotate_z"; 536 | move_control.orientation.x = 0.0; 537 | move_control.orientation.y = 1.0; 538 | move_control.orientation.z = 0.0; 539 | move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 540 | wp_itr_marker.controls.push_back(move_control); 541 | 542 | // 加入菜单 543 | visualization_msgs::Marker menu_marker; 544 | menu_marker.type = visualization_msgs::Marker::CUBE; 545 | menu_marker.scale.x = 0.5; 546 | menu_marker.scale.y = 0.5; 547 | menu_marker.scale.z = 0.5; 548 | menu_marker.color.r = 0.9; 549 | menu_marker.color.g = 0.9; 550 | menu_marker.color.b = 0.9; 551 | menu_marker.color.a = 0.0; //全透明 552 | InteractiveMarkerControl menu_control; 553 | menu_control.interaction_mode = InteractiveMarkerControl::BUTTON; 554 | menu_control.always_visible = true; 555 | menu_control.markers.push_back( menu_marker ); 556 | wp_itr_marker.controls.push_back( menu_control ); 557 | 558 | inServer->insert(wp_itr_marker, &processChargerFeedback); 559 | } 560 | 561 | // 菜单删除航点的回调函数 562 | void DeleteWaypointCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 563 | { 564 | strDelWaypointName = feedback->marker_name; 565 | bDeleteWaypoint = true; 566 | ROS_WARN("Menu - Delete waypoint %s",strDelWaypointName.c_str()); 567 | } 568 | 569 | // 菜单删除充电桩的回调函数 570 | void DeleteChargerCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ) 571 | { 572 | strDelChargerName = feedback->marker_name; 573 | bDeleteCharger = true; 574 | ROS_WARN("Menu - Delete charger %s",strDelChargerName.c_str()); 575 | } 576 | 577 | // 添加新航点回调函数 578 | void AddWayPointCallback(const waterplus_map_tools::Waypoint::ConstPtr& wp) 579 | { 580 | ROS_INFO("Add_waypoint: %s (%.2f %.2f) (%.2f %.2f %.2f %.2f) ",wp->name.c_str(), wp->pose.position.x, wp->pose.position.y, wp->pose.orientation.x, wp->pose.orientation.y, wp->pose.orientation.z, wp->pose.orientation.w); 581 | waterplus_map_tools::Waypoint newWayPoint; 582 | newWayPoint = *wp; 583 | int nWPNum = arWaypoint.size(); 584 | for(int i= 0;iapply( *pWaypointServer, newWayPoint.name ); 596 | //通知client（RVIZ）更新显示 597 | pWaypointServer->applyChanges(); 598 | } 599 | } 600 | 601 | // 添加新充电桩回调函数 602 | void AddChargerCallback(const waterplus_map_tools::Waypoint::ConstPtr& wp) 603 | { 604 | ROS_INFO("Add_charger: %s (%.2f %.2f) (%.2f %.2f %.2f %.2f) ",wp->name.c_str(), wp->pose.position.x, wp->pose.position.y, wp->pose.orientation.x, wp->pose.orientation.y, wp->pose.orientation.z, wp->pose.orientation.w); 605 | waterplus_map_tools::Waypoint newCharger; 606 | newCharger = *wp; 607 | int nChargerNum = arCharger.size(); 608 | for(int i= 0;iapply( *pWaypointServer, newCharger.name ); 620 | //通知client（RVIZ）更新显示 621 | pWaypointServer->applyChanges(); 622 | } 623 | } 624 | 625 | int main(int argc, char** argv) 626 | { 627 | ros::init(argc, argv, "wp_edit_node"); 628 | 629 | std::string strLoadFile; 630 | char const* home = getenv("HOME"); 631 | strLoadFile = home; 632 | strLoadFile += "/waypoints.xml"; 633 | 634 | ros::NodeHandle n_param("~"); 635 | std::string strParamFile; 636 | n_param.param("load", strParamFile, ""); 637 | if(strParamFile.length() > 0) 638 | { 639 | strLoadFile = strParamFile; 640 | } 641 | 642 | if(strLoadFile.length() > 0) 643 | { 644 | ROS_INFO("Load waypoints from file : %s",strLoadFile.c_str()); 645 | LoadWaypointsFromFile(strLoadFile); 646 | } 647 | else 648 | { 649 | ROS_WARN("strLoadFile is empty. Failed to load waypoints!"); 650 | } 651 | 652 | ros::NodeHandle nh; 653 | 654 | //创建服务 655 | interactive_markers::InteractiveMarkerServer wp_server("waypoints_move"); 656 | pWaypointServer = &wp_server; 657 | 658 | marker_pub = nh.advertise("text_marker", 100); 659 | ros::Subscriber add_waypoint_sub = nh.subscribe("waterplus/add_waypoint",10,&AddWayPointCallback); 660 | ros::Subscriber add_charger_sub = nh.subscribe("waterplus/add_charger",10,&AddChargerCallback); 661 | ros::ServiceServer srvGetNum = nh.advertiseService("waterplus/get_num_waypoint", getNumOfWaypoints); 662 | ros::ServiceServer srvGetWPIndex = nh.advertiseService("waterplus/get_waypoint_index", getWaypointByIndex); 663 | ros::ServiceServer srvGetWPName = nh.advertiseService("waterplus/get_waypoint_name", getWaypointByName); 664 | ros::ServiceServer srvSaveWP = nh.advertiseService("waterplus/save_waypoints", saveWaypoints); 665 | ros::ServiceServer srvGetChargerName = nh.advertiseService("waterplus/get_charger_name", getChargerByName); 666 | 667 | //将互动标记放到标记集合里，同时指定Feedback回调函数 668 | int nWPNum = arWaypoint.size(); 669 | ROS_INFO("Num of waypoints = %d",nWPNum); 670 | for(int i=0; i< nWPNum; i++) 671 | { 672 | NewWaypointInterMarker( &wp_server, arWaypoint[i].name, arWaypoint[i].pose ); 673 | } 674 | MenuHandler menu_waypoint; 675 | menu_waypoint.insert( "Delete", &DeleteWaypointCallback); 676 | pMenuWaypoint = &menu_waypoint; 677 | for(int i=0; i< nWPNum; i++) 678 | { 679 | menu_waypoint.apply( wp_server, arWaypoint[i].name ); 680 | } 681 | // 充电桩标记初始化 682 | int nChargerNum = arCharger.size(); 683 | ROS_INFO("Num of chargers = %d",nChargerNum); 684 | for(int i=0; i< nChargerNum; i++) 685 | { 686 | NewChargerInterMarker( &wp_server, arCharger[i].name, arCharger[i].pose ); 687 | } 688 | MenuHandler menu_charger; 689 | menu_charger.insert( "Delete", &DeleteChargerCallback); 690 | pMenuCharger = &menu_charger; 691 | for(int i=0; i< nChargerNum; i++) 692 | { 693 | menu_charger.apply( wp_server, arCharger[i].name ); 694 | } 695 | 696 | //通知client（RVIZ）更新显示 697 | wp_server.applyChanges(); 698 | 699 | ros::Rate r(10); 700 | 701 | while(ros::ok()) 702 | { 703 | if(bDeleteWaypoint == true) 704 | { 705 | bDeleteWaypoint = false; 706 | wp_server.erase(strDelWaypointName); 707 | wp_server.applyChanges(); 708 | for(vector::iterator iter=arWaypoint.begin(); iter!=arWaypoint.end(); ) 709 | { 710 | if( (*iter).name == strDelWaypointName) 711 | iter = arWaypoint.erase(iter); 712 | else 713 | iter ++ ; 714 | } 715 | ROS_WARN("%s waypoint deleted!",strDelWaypointName.c_str()); 716 | RemoveTextMarker(); 717 | } 718 | if(bDeleteCharger == true) 719 | { 720 | bDeleteCharger = false; 721 | wp_server.erase(strDelChargerName); 722 | wp_server.applyChanges(); 723 | for(vector::iterator iter=arCharger.begin(); iter!=arCharger.end(); ) 724 | { 725 | if( (*iter).name == strDelChargerName) 726 | iter = arCharger.erase(iter); 727 | else 728 | iter ++ ; 729 | } 730 | ROS_WARN("%s charger deleted!",strDelChargerName.c_str()); 731 | RemoveTextMarker(); 732 | } 733 | PublishTextMarker(); 734 | ros::spinOnce(); 735 | r.sleep(); 736 | } 737 | 738 | return 0; 739 | } --------------------------------------------------------------------------------