├── .gitignore ├── srv └── Shutdown.srv ├── document └── 开发总结 ├── launch ├── fake.launch └── xqserial.launch ├── LICENSE ├── include ├── DiffDriverController.h ├── StatusPublisher.h └── AsyncSerial.h ├── README.md ├── CMakeLists.txt ├── package.xml ├── src ├── main.cpp ├── xqserial_server │ └── fake_node.py ├── DiffDriverController.cpp ├── StatusPublisher.cpp └── AsyncSerial.cpp ├── .vscode └── c_cpp_properties.json ├── script └── visualization.py └── CMakeLists.txt.user /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | *~ 3 | .vscode -------------------------------------------------------------------------------- /srv/Shutdown.srv: -------------------------------------------------------------------------------- 1 | bool flag 2 | --- 3 | bool result -------------------------------------------------------------------------------- /document/开发总结: -------------------------------------------------------------------------------- 1 | 建立包架构 2 | catkin_create_pkg xqserial_server std_msgs rospy roscpp nav_msgs sensor_msgs 3 | 编译 4 | catkin_make xqserial_server 5 | 6 | c++总结： 7 | 1.cout 需要endl刷新 8 | 2.ros::init(argc, argv, "xqserial_server");中的argv是二级指针 9 | 3.使用ros::param::param("~baud", baud, 115200);这种写法获取baud参数时，debug时提供的参数列表为 _baud:=9600 ，请留意～与_ 10 | -------------------------------------------------------------------------------- /launch/fake.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/xqserial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Bluewhale Robot 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /include/DiffDriverController.h: -------------------------------------------------------------------------------- 1 | #ifndef DIFFDRIVERCONTROLLER_H 2 | #define DIFFDRIVERCONTROLLER_H 3 | #include 4 | #include "StatusPublisher.h" 5 | #include "AsyncSerial.h" 6 | #include 7 | #include "galileo_serial_server/GalileoStatus.h" 8 | 9 | namespace xqserial_server 10 | { 11 | 12 | class DiffDriverController 13 | { 14 | public: 15 | DiffDriverController(); 16 | DiffDriverController(double max_speed_, std::string cmd_topic_, StatusPublisher *xq_status_, CallbackAsyncSerial *cmd_serial_,double r_min); 17 | void run(); 18 | void sendcmd(const geometry_msgs::Twist &command); 19 | void imuCalibration(const std_msgs::Bool &calFlag); 20 | void setStatusPtr(StatusPublisher &status); 21 | void updateMoveFlag(const std_msgs::Bool &moveFlag); 22 | void updateBarDetectFlag(const std_msgs::Bool &DetectFlag); 23 | void UpdateNavStatus(const galileo_serial_server::GalileoStatus& current_receive_status); 24 | 25 | private: 26 | double max_wheelspeed; //单位为转每秒,只能为正数 27 | std::string cmd_topic; 28 | StatusPublisher *xq_status; 29 | CallbackAsyncSerial *cmd_serial; 30 | boost::mutex mMutex; 31 | bool MoveFlag; 32 | boost::mutex mStausMutex_; 33 | galileo_serial_server::GalileoStatus galileoStatus_; 34 | float R_min_; 35 | }; 36 | 37 | } // namespace xqserial_server 38 | #endif // DIFFDRIVERCONTROLLER_H 39 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # xqserial_server 2 | xiaoqiang motor driver and low level ROS api package 3 | 4 | ## input topic 5 | name type 6 | /cmd_vel geometry_msgs/Twist 7 | /barDetectFlag std_msgs/Bool 8 | 9 | ## output topic 10 | name type frate 11 | /xqserial_server/Odom nav_msgs/Odometry 50hz 12 | /xqserial_server/Pose2D geometry_msgs/Pose2D 50hz 13 | /xqserial_server/Power std_msgs/Float64 50hz 14 | /xqserial_server/StatusFlag std_msgs/Int32 50hz 15 | /xqserial_server/Twist geometry_msgs/Twist 50hz 16 | /tf odom-->base_footprint 50hz 17 | /tf_static base_footprint-->base_link 100hz 18 | 19 | ## input param 20 | name default 21 | port /dev/ttyUSB0 22 | baud 115200 23 | wheel_separation 0.37 24 | wheel_radius 0.06 25 | debug_flag false 26 | max_speed 2.0 27 | cmd_topic cmd_vel 28 | 29 | ## Usage: 30 | ### download to xiaoqiang ros workspace 31 | ``` 32 | cd ~/Documents/ros/src 33 | git clone https://github.com/BlueWhaleRobot/xqserial_server.git 34 | cd .. 35 | catkin_make 36 | ``` 37 | ### Quickstart 38 | ``` 39 | roslaunch xqserial_server xqserial.launch 40 | ``` 41 | ## Made with :heart: by BlueWhale Tech corp. 42 | 43 | 44 | 小强电机驱动和相关的ROS底层驱动程序。 45 | ## 使用方法： 46 | #### 安装到小强ROS工作目录 47 | ``` 48 | cd ~/Documents/ros/src 49 | git clone https://github.com/BlueWhaleRobot/xqserial_server.git 50 | cd .. 51 | catkin_make 52 | ``` 53 | ### 直接启动 54 | ``` 55 | roslaunch xqserial_server xqserial.launch 56 | ``` 57 | 58 | ## 由蓝鲸科技精 :heart: 制作。 59 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(xqserial_server) 3 | 4 | include(CheckCXXCompilerFlag) 5 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 6 | if(COMPILER_SUPPORTS_CXX11) 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 8 | add_definitions(-DCOMPILEDWITHC11) 9 | message(STATUS "Using flag -std=c++11.") 10 | else() 11 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 12 | endif() 13 | 14 | find_package(catkin REQUIRED COMPONENTS 15 | nav_msgs 16 | roscpp 17 | rospy 18 | sensor_msgs 19 | std_msgs 20 | geometry_msgs 21 | tf 22 | xiaoqiang_log 23 | galileo_serial_server 24 | ) 25 | 26 | add_service_files( 27 | FILES 28 | Shutdown.srv 29 | ) 30 | 31 | generate_messages( 32 | DEPENDENCIES 33 | std_msgs 34 | ) 35 | 36 | set(BOOST_LIBS thread date_time system) 37 | find_package(Boost COMPONENTS ${BOOST_LIBS} REQUIRED) 38 | find_package(Threads REQUIRED) 39 | find_package(PkgConfig REQUIRED) 40 | 41 | pkg_check_modules(JSONCPP REQUIRED jsoncpp) 42 | 43 | catkin_package( 44 | INCLUDE_DIRS include 45 | CATKIN_DEPENDS nav_msgs roscpp rospy sensor_msgs std_msgs geometry_msgs xiaoqiang_log 46 | ) 47 | 48 | include_directories( 49 | include 50 | ${Boost_INCLUDE_DIRS} 51 | ${catkin_INCLUDE_DIRS} 52 | ${JSONCPP_INCLUDE_DIRS} 53 | ) 54 | 55 | set(SRCS src/main.cpp src/AsyncSerial.cpp src/DiffDriverController.cpp src/StatusPublisher.cpp) 56 | add_executable(${PROJECT_NAME} ${SRCS}) 57 | add_dependencies( 58 | ${PROJECT_NAME} 59 | ${catkin_EXPORTED_TARGETS} xiaoqiang_log_cpp 60 | ) 61 | 62 | target_link_libraries(${PROJECT_NAME} 63 | ${catkin_LIBRARIES} 64 | ${JSONCPP_LIBRARIES} 65 | ) 66 | set_target_properties(${PROJECT_NAME} PROPERTIES OUTPUT_NAME ${PROJECT_NAME} PREFIX "") 67 | 68 | install( 69 | TARGETS ${PROJECT_NAME} ${PROJECT_NAME} 70 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 71 | ) 72 | 73 | install( 74 | DIRECTORY launch 75 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 76 | ) 77 | 78 | install( 79 | DIRECTORY include/${PROJECT_NAME}/ 80 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 81 | ) 82 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | xqserial_server 4 | 1.0.1 5 | The xqserial_server package 6 | 7 | 8 | 9 | 10 | frank 11 | 12 | 13 | 14 | 15 | 16 | BSD 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 | catkin 43 | nav_msgs 44 | roscpp 45 | rospy 46 | sensor_msgs 47 | std_msgs 48 | geometry_msgs 49 | tf 50 | libjsoncpp 51 | xiaoqiang_log 52 | galileo_serial_server 53 | message_generation 54 | 55 | nav_msgs 56 | roscpp 57 | rospy 58 | sensor_msgs 59 | std_msgs 60 | geometry_msgs 61 | tf 62 | libjsoncpp 63 | xiaoqiang_log 64 | galileo_serial_server 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /include/StatusPublisher.h: -------------------------------------------------------------------------------- 1 | #ifndef STATUSPUBLISHER_H 2 | #define STATUSPUBLISHER_H 3 | 4 | #include "ros/ros.h" 5 | #include 6 | #include 7 | #include 8 | #include "geometry_msgs/Pose2D.h" 9 | #include "geometry_msgs/Twist.h" 10 | #include "geometry_msgs/Vector3.h" 11 | #include "nav_msgs/Odometry.h" 12 | #include "std_msgs/Int32.h" 13 | #include "std_msgs/Float64.h" 14 | #include "tf/transform_broadcaster.h" 15 | #include 16 | #include 17 | #include 18 | 19 | #define PI 3.14159265 20 | 21 | namespace xqserial_server 22 | { 23 | typedef struct 24 | { 25 | int status; //小车状态，0表示未初始化，1表示正常，-1表示error 26 | float power; //电源电压【9 13】v 27 | float theta; //方位角，【0 360）° 28 | int encoder_ppr; //车轮1转对应的编码器个数 29 | int encoder_delta_r; //右轮编码器增量，个为单位 30 | int encoder_delta_l; //左轮编码器增量，个为单位 31 | int encoder_delta_car; //两车轮中心位移，个为单位 32 | int omga_r; //右轮转速个每秒 33 | int omga_l; //左轮转速个每秒 34 | float distance1; //第一个超声模块距离值单位cm 35 | float distance2; //第二个超声模块距离值单位cm 36 | float distance3; //第三个超声模块距离值单位cm 37 | float distance4; //第四个超声模块距离值单位cm 38 | float IMU[9]; //mpu9250 9轴数据 39 | unsigned int time_stamp; //时间戳 40 | } UPLOAD_STATUS; 41 | 42 | class StatusPublisher 43 | { 44 | 45 | public: 46 | StatusPublisher(); 47 | StatusPublisher(double separation, double radius); 48 | void Refresh(); 49 | void Update(const char *data, unsigned int len); 50 | double get_wheel_separation(); 51 | double get_wheel_radius(); 52 | int get_wheel_ppr(); 53 | int get_status(); 54 | geometry_msgs::Pose2D get_CarPos2D(); 55 | void get_wheel_speed(double speed[2]); 56 | geometry_msgs::Twist get_CarTwist(); 57 | std_msgs::Float64 get_power(); 58 | nav_msgs::Odometry get_odom(); 59 | UPLOAD_STATUS car_status; 60 | 61 | private: 62 | //Wheel separation, wrt the midpoint of the wheel width: meters 63 | double wheel_separation; 64 | 65 | //Wheel radius (assuming it's the same for the left and right wheels):meters 66 | double wheel_radius; 67 | 68 | geometry_msgs::Pose2D CarPos2D; // 小车开始启动原点坐标系 69 | geometry_msgs::Twist CarTwist; // 小车自身坐标系 70 | std_msgs::Float64 CarPower; // 小车电池信息 71 | nav_msgs::Odometry CarOdom; // 小车位置和速度信息 72 | ros::NodeHandle mNH; 73 | ros::Publisher mPose2DPub; 74 | ros::Publisher mTwistPub; 75 | ros::Publisher mStatusFlagPub; 76 | ros::Publisher mPowerPub; 77 | ros::Publisher mOdomPub; 78 | ros::Publisher pub_barpoint_cloud_; 79 | ros::Publisher pub_clearpoint_cloud_; 80 | 81 | bool mbUpdated; 82 | 83 | boost::mutex mMutex; 84 | double base_time_; 85 | 86 | ros::Publisher mIMUPub; 87 | sensor_msgs::Imu CarIMU; 88 | }; 89 | 90 | } //namespace xqserial_server 91 | 92 | #endif // STATUSPUBLISHER_H 93 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "AsyncSerial.h" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include "DiffDriverController.h" 12 | #include "StatusPublisher.h" 13 | #include "xiaoqiang_log/LogRecord.h" 14 | 15 | using namespace std; 16 | 17 | inline bool exists(const std::string &name) 18 | { 19 | struct stat buffer; 20 | return (stat(name.c_str(), &buffer) == 0); 21 | } 22 | 23 | int main(int argc, char **argv) 24 | { 25 | cout << "welcome to xiaoqiang serial server,please feel free at home!" << endl; 26 | 27 | ros::init(argc, argv, "xqserial_server"); 28 | ros::start(); 29 | 30 | //获取串口参数 31 | std::string port; 32 | ros::param::param("~port", port, "/dev/ttyUSB0"); 33 | int baud; 34 | ros::param::param("~baud", baud, 115200); 35 | cout << "port:" << port << " baud:" << baud << endl; 36 | //获取小车机械参数 37 | double separation = 0, radius = 0; 38 | bool DebugFlag = false; 39 | ros::param::param("~wheel_separation", separation, 0.37); 40 | ros::param::param("~wheel_radius", radius, 0.0625); 41 | ros::param::param("~debug_flag", DebugFlag, false); 42 | xqserial_server::StatusPublisher xq_status(separation, radius); 43 | 44 | //获取小车控制参数 45 | double max_speed,r_min; 46 | string cmd_topic; 47 | ros::param::param("~max_speed", max_speed, 2.0); 48 | ros::param::param("~r_min", r_min, 0.25); 49 | ros::param::param("~cmd_topic", cmd_topic, "cmd_vel"); 50 | 51 | // 初始化log发布者和语音发布者 52 | ros::NodeHandle mNH; 53 | ros::Publisher log_pub = mNH.advertise("/xiaoqiang_log", 1, true); 54 | ros::Publisher audio_pub = mNH.advertise("/xiaoqiang_tts/text", 1, true); 55 | 56 | try 57 | { 58 | CallbackAsyncSerial serial(port, baud); 59 | serial.setCallback(boost::bind(&xqserial_server::StatusPublisher::Update, &xq_status, _1, _2)); 60 | xqserial_server::DiffDriverController xq_diffdriver(max_speed, cmd_topic, &xq_status, &serial,r_min); 61 | boost::thread cmd2serialThread(&xqserial_server::DiffDriverController::run, &xq_diffdriver); 62 | // send test flag 63 | char debugFlagCmd[] = {(char)0xcd, (char)0xeb, (char)0xd7, (char)0x01, 'T'}; 64 | if (DebugFlag) 65 | { 66 | ROS_INFO_STREAM("Debug mode Enabled"); 67 | serial.write(debugFlagCmd, 5); 68 | } 69 | // send reset cmd 70 | char resetCmd[] = {(char)0xcd, (char)0xeb, (char)0xd7, (char)0x01, 'I'}; 71 | serial.write(resetCmd, 5); 72 | ros::Duration(0.5).sleep(); 73 | 74 | ros::Rate r(100); //发布周期为50hz 75 | 76 | while (ros::ok()) 77 | { 78 | if (serial.errorStatus() || serial.isOpen() == false) 79 | { 80 | ROS_ERROR_STREAM("Error: serial port closed unexpectedly"); 81 | break; 82 | } 83 | xq_status.Refresh(); //定时发布状态 84 | r.sleep(); 85 | //cout<<"run"< base_footprint 的tf变换 15 | 订阅消息 16 | /cmd_vel 17 | /global_move_flag 18 | /xqserial_server/initPose 19 | """ 20 | 21 | import threading 22 | import rospy 23 | from geometry_msgs.msg import Pose2D, Twist, Quaternion 24 | from nav_msgs.msg import Odometry 25 | from std_msgs.msg import Float64, Int32, Bool 26 | from sensor_msgs.msg import Imu 27 | import tf 28 | from tf.transformations import quaternion_from_euler 29 | import math 30 | 31 | CURRENT_TWIST = Twist() 32 | MOVE_FLAG = Bool() 33 | MOVE_FLAG.data = True 34 | CURRENT_ODOM = Odometry() 35 | CURRENT_POSE2D = Pose2D() 36 | DATA_LOCK = threading.RLock() 37 | CURRENT_IMU = Imu() 38 | CURRENT_POWER = Float64() 39 | CURRENT_POWER.data = 12 40 | 41 | def update_current_speed(twist): 42 | global CURRENT_TWIST, DATA_LOCK 43 | with DATA_LOCK: 44 | if twist.linear.x > 10 or twist.linear.y > 10 or twist.linear.x > 10 or \ 45 | twist.angular.x > 10 or twist.angular.y > 10 or twist.angular.z > 10: 46 | rospy.logerr("Invalid twist data received") 47 | return 48 | CURRENT_TWIST.linear.x = twist.linear.x 49 | CURRENT_TWIST.linear.y = twist.linear.y 50 | CURRENT_TWIST.linear.z = twist.linear.z 51 | CURRENT_TWIST.angular.x = twist.angular.x 52 | CURRENT_TWIST.angular.y = twist.angular.y 53 | CURRENT_TWIST.angular.z = twist.angular.z 54 | 55 | def update_move_flag(flag): 56 | global MOVE_FLAG, DATA_LOCK 57 | with DATA_LOCK: 58 | MOVE_FLAG.data = flag.data 59 | 60 | def update_location(): 61 | global DATA_LOCK, CURRENT_ODOM, MOVE_FLAG, CURRENT_TWIST, CURRENT_POSE2D, CURRENT_IMU, CURRENT_POWER 62 | rate = rospy.Rate(50) 63 | odom_pub = rospy.Publisher("/xqserial_server/Odom", Odometry, queue_size=0) 64 | pose_pub = rospy.Publisher("/xqserial_server/Pose2D", Pose2D, queue_size=0) 65 | twist_pub = rospy.Publisher("/xqserial_server/Twist", Twist, queue_size=0) 66 | power_pub = rospy.Publisher( 67 | "/xqserial_server/Power", Float64, queue_size=0) 68 | imu_pub = rospy.Publisher("/xqserial_server/IMU", Imu, queue_size=0) 69 | status_flag_pub = rospy.Publisher("/xqserial_server/StatusFlag", Int32, 70 | queue_size=0) 71 | odom_tf = tf.TransformBroadcaster() 72 | while not rospy.is_shutdown(): 73 | with DATA_LOCK: 74 | if MOVE_FLAG.data: 75 | # 更新当前位置 76 | if CURRENT_TWIST.linear.x > 10: 77 | rospy.logerr("Invalid current twist") 78 | CURRENT_POSE2D.x += CURRENT_TWIST.linear.x * 0.02 * math.cos( CURRENT_POSE2D.theta * math.pi / 180 ) 79 | CURRENT_POSE2D.y += CURRENT_TWIST.linear.x * 0.02 * math.sin( CURRENT_POSE2D.theta * math.pi / 180 ) 80 | CURRENT_POSE2D.theta += CURRENT_TWIST.angular.z * 0.02 * 180 / math.pi 81 | if CURRENT_POSE2D.x > 1000 or CURRENT_POSE2D.y > 1000: 82 | rospy.logerr("Invalid current pose") 83 | if CURRENT_POSE2D.theta > 360: 84 | CURRENT_POSE2D.theta -= 360 85 | if CURRENT_POSE2D.theta < 0: 86 | CURRENT_POSE2D.theta += 360 87 | pose_pub.publish(CURRENT_POSE2D) 88 | # 更新odom 89 | CURRENT_ODOM.header.stamp = rospy.Time.now() 90 | CURRENT_ODOM.header.frame_id = "odom" 91 | CURRENT_ODOM.pose.pose.position.x = CURRENT_POSE2D.x 92 | CURRENT_ODOM.pose.pose.position.y = CURRENT_POSE2D.y 93 | CURRENT_ODOM.pose.pose.position.z = 0 94 | q_angle = quaternion_from_euler(0, 0, CURRENT_POSE2D.theta * 3.14 / 180, axes='sxyz') 95 | q = Quaternion(*q_angle) 96 | CURRENT_ODOM.pose.pose.orientation = q 97 | CURRENT_ODOM.twist.twist = CURRENT_TWIST 98 | odom_pub.publish(CURRENT_ODOM) 99 | else: 100 | pose_pub.publish(CURRENT_POSE2D) 101 | CURRENT_ODOM.header.stamp = rospy.Time.now() 102 | CURRENT_ODOM.header.frame_id = "odom" 103 | odom_pub.publish(CURRENT_ODOM) 104 | # 更新IMU数据 105 | CURRENT_IMU.header.frame_id = "imu" 106 | CURRENT_IMU.header.stamp = rospy.Time.now() 107 | q_angle = quaternion_from_euler(0, 0, CURRENT_POSE2D.theta * 3.14 / 180, axes='sxyz') 108 | q = Quaternion(*q_angle) 109 | CURRENT_IMU.orientation = q 110 | CURRENT_IMU.angular_velocity.x = 0 111 | CURRENT_IMU.angular_velocity.y = 0 112 | CURRENT_IMU.angular_velocity.z = CURRENT_TWIST.angular.z 113 | CURRENT_IMU.linear_acceleration.x = 0 114 | CURRENT_IMU.linear_acceleration.y = 0 115 | CURRENT_IMU.linear_acceleration.z = 9.8 116 | imu_pub.publish(CURRENT_IMU) 117 | # 更新电压, 每秒下降0.01v用于测试自动充电程序 118 | current_power = None 119 | with DATA_LOCK: 120 | current_power = CURRENT_POWER 121 | current_power.data -= 0.01 / 50 122 | if current_power.data < 10: 123 | current_power.data = 10 124 | power_pub.publish(current_power) 125 | # 更新statusFlag 126 | current_status = Int32() 127 | current_status.data = 0 128 | status_flag_pub.publish(current_status) 129 | # 发布 Twist 130 | twist_pub.publish(CURRENT_TWIST) 131 | # 发布 base_footprint到odom的tf 132 | odom_tf.sendTransform((CURRENT_POSE2D.x, CURRENT_POSE2D.y, 0), 133 | tf.transformations.quaternion_from_euler(0, 0, CURRENT_POSE2D.theta * math.pi / 180), 134 | rospy.Time.now(), 135 | "base_footprint", 136 | "odom" 137 | ) 138 | rate.sleep() 139 | 140 | 141 | def update_power(power): 142 | # 设置电压话题，用于虚拟自动充电节点更新电压 143 | global CURRENT_POWER, DATA_LOCK 144 | with DATA_LOCK: 145 | CURRENT_POWER.data = power.data 146 | 147 | if __name__ == "__main__": 148 | rospy.init_node("xqserial_server", anonymous=True) 149 | rospy.Subscriber("/cmd_vel", Twist, update_current_speed) 150 | rospy.Subscriber("/global_move_flag", Bool, update_move_flag) 151 | rospy.Subscriber("/set_power", Float64, update_power) 152 | update_location() 153 | 154 | 155 | 156 | -------------------------------------------------------------------------------- /script/visualization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding: UTF-8 3 | import roslib 4 | import rospy 5 | from visual import * 6 | import math 7 | import wx 8 | 9 | from sensor_msgs.msg import Imu 10 | from tf.transformations import euler_from_quaternion 11 | 12 | rad2degrees = 180.0/math.pi 13 | precision = 2 #round to this number of digits 14 | yaw_offset = 0 #used to align animation upon key press 15 | 16 | 17 | #Create shutdown hook to kill visual displays 18 | def shutdown_hook(): 19 | #print "Killing displays" 20 | wx.Exit() 21 | 22 | #register shutdown hook 23 | rospy.on_shutdown(shutdown_hook) 24 | 25 | 26 | # Main scene 27 | scene=display(title="9DOF Razor IMU Main Screen") 28 | scene.range=(1.3,1.3,1.3) 29 | scene.forward = (-1,0,-0.25)#(1,0,-0.25) 30 | # Change reference axis (x,y,z) - z is up 31 | scene.up=(0,0,1) 32 | scene.width=500 33 | scene.height=500 34 | 35 | # Second scene (Roll, Pitch, Yaw) 36 | scene2 = display(title='9DOF Razor IMU Roll, Pitch, Yaw',x=550, y=0, width=500, height=500,center=(0,0,0), background=(0,0,0)) 37 | scene2.range=(1,1,1) 38 | scene2.select() 39 | #Roll, Pitch, Yaw 40 | #Default reference, i.e. x runs right, y runs up, z runs backward (out of screen) 41 | cil_roll = cylinder(pos=(-0.5,0.3,0),axis=(0.2,0,0),radius=0.01,color=color.red) 42 | cil_roll2 = cylinder(pos=(-0.5,0.3,0),axis=(-0.2,0,0),radius=0.01,color=color.red) 43 | cil_pitch = arrow(pos=(0.5,0.3,0),axis=(0,0,-0.4),shaftwidth=0.02,color=color.green) 44 | arrow_course = arrow(pos=(0.0,-0.4,0),color=color.cyan,axis=(0,0.2,0), shaftwidth=0.02, fixedwidth=1) 45 | 46 | #Roll,Pitch,Yaw labels 47 | label(pos=(-0.5,0.6,0),text="Roll (degrees / radians)",box=0,opacity=0) 48 | label(pos=(0.5,0.6,0),text="Pitch (degrees / radians)",box=0,opacity=0) 49 | label(pos=(0.0,0.02,0),text="Yaw (degrees / radians)",box=0,opacity=0) 50 | label(pos=(0.0,-0.16,0),text="N",box=0,opacity=0,color=color.yellow) 51 | label(pos=(0.0,-0.64,0),text="S",box=0,opacity=0,color=color.yellow) 52 | label(pos=(-0.24,-0.4,0),text="W",box=0,opacity=0,color=color.yellow) 53 | label(pos=(0.24,-0.4,0),text="E",box=0,opacity=0,color=color.yellow) 54 | label(pos=(0.18,-0.22,0),height=7,text="NE",box=0,color=color.yellow) 55 | label(pos=(-0.18,-0.22,0),height=7,text="NW",box=0,color=color.yellow) 56 | label(pos=(0.18,-0.58,0),height=7,text="SE",box=0,color=color.yellow) 57 | label(pos=(-0.18,-0.58,0),height=7,text="SW",box=0,color=color.yellow) 58 | 59 | rollLabel = label(pos=(-0.5,0.52,0),text="-",box=0,opacity=0,height=12) 60 | pitchLabel = label(pos=(0.5,0.52,0),text="-",box=0,opacity=0,height=12) 61 | yawLabel = label(pos=(0,-0.06,0),text="-",box=0,opacity=0,height=12) 62 | 63 | #acceleration labels 64 | label(pos=(0,0.9,0),text="Linear Acceleration x / y / z (m/s^2)",box=0,opacity=0) 65 | label(pos=(0,-0.8,0),text="Angular Velocity x / y / z (rad/s)",box=0,opacity=0) 66 | linAccLabel = label(pos=(0,0.82,0),text="-",box=0,opacity=0,height=12) 67 | angVelLabel = label(pos=(0,-0.88,0),text="-",box=0,opacity=0,height=12) 68 | 69 | # Main scene objects 70 | scene.select() 71 | # Reference axis (x,y,z) - using ROS conventions (REP 103) - z is up, y left (west, 90 deg), x is forward (north, 0 deg) 72 | # In visual, z runs up, x runs forward, y runs left (see scene.up command earlier) 73 | # So everything is good 74 | arrow(color=color.green,axis=(1,0,0), shaftwidth=0.04, fixedwidth=1) 75 | arrow(color=color.green,axis=(0,1,0), shaftwidth=0.04 , fixedwidth=1) 76 | arrow(color=color.green,axis=(0,0,1), shaftwidth=0.04, fixedwidth=1) 77 | 78 | # labels 79 | label(pos=(0,0,-1.2),text="Press 'a' to align",box=0,opacity=0) 80 | label(pos=(1,0.1,0),text="X",box=0,opacity=0) 81 | label(pos=(0,1,-0.1),text="Y",box=0,opacity=0) 82 | label(pos=(0,-0.1,1),text="Z",box=0,opacity=0) 83 | # IMU object 84 | platform = box(length=0.65, height=0.05, width=1.5, color=color.red,up=(0,0,1),axis=(-1,0,0)) 85 | p_line = box(length=1.1,height=0.08,width=0.1,color=color.yellow,up=(0,0,1),axis=(-1,0,0)) 86 | plat_arrow = arrow(length=-0.8,color=color.cyan,up=(0,0,1),axis=(-1,0,0), shaftwidth=0.06, fixedwidth=1) 87 | plat_arrow_up = arrow(length=0.4,color=color.cyan,up=(-1,0,0),axis=(0,0,1), shaftwidth=0.06, fixedwidth=1) 88 | rospy.init_node("display_3D_visualization_node") 89 | 90 | def processIMU_message(imuMsg): 91 | global yaw_offset 92 | 93 | roll=0 94 | pitch=0 95 | yaw=0 96 | 97 | quaternion = ( 98 | imuMsg.orientation.x, 99 | imuMsg.orientation.y, 100 | imuMsg.orientation.z, 101 | imuMsg.orientation.w) 102 | (roll,pitch,yaw) = euler_from_quaternion(quaternion) 103 | 104 | #add align offset to yaw 105 | yaw += yaw_offset 106 | 107 | axis=(-cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) 108 | up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw),-sin(roll)*cos(yaw)+cos(roll)*sin(pitch)*sin(yaw),cos(roll)*cos(pitch)) 109 | platform.axis=axis 110 | platform.up=up 111 | platform.length=1.0 112 | plat_arrow_up.axis=up 113 | plat_arrow_up.up=axis 114 | plat_arrow_up.length=0.4 115 | plat_arrow.axis=axis 116 | plat_arrow.up=up 117 | plat_arrow.length=-0.8 118 | p_line.axis=axis 119 | p_line.up=up 120 | p_line.length=1.1 121 | cil_roll.axis=(-0.2*cos(roll),0.2*sin(roll),0) 122 | cil_roll2.axis=(0.2*cos(roll),-0.2*sin(roll),0) 123 | cil_pitch.axis=(0,-0.4*sin(pitch),-0.4*cos(pitch)) 124 | #remove yaw_offset from yaw display 125 | arrow_course.axis=(-0.2*sin(yaw-yaw_offset),0.2*cos(yaw-yaw_offset),0) 126 | 127 | #display in degrees / radians 128 | rollLabel.text = str(round(roll*rad2degrees, precision)) + " / " + str(round(roll,precision)) 129 | pitchLabel.text = str(round(pitch*rad2degrees, precision)) + " / " + str(round(pitch, precision)) 130 | #remove yaw_offset from yaw display 131 | yawLabel.text = str(round((yaw-yaw_offset)*rad2degrees, precision)) + " / " + str(round((yaw-yaw_offset), precision)) 132 | 133 | linAccLabel.text = str(round(imuMsg.linear_acceleration.x, precision)) + " / " + str(round(imuMsg.linear_acceleration.y, precision)) + " / " + str(round(imuMsg.linear_acceleration.z, precision)) 134 | angVelLabel.text = str(round(imuMsg.angular_velocity.x, precision)) + " / " + str(round(imuMsg.angular_velocity.y, precision)) + " / " + str(round(imuMsg.angular_velocity.z, precision)) 135 | 136 | #check for align key press - if pressed, next refresh will be aligned 137 | if scene.kb.keys: # event waiting to be processed? 138 | s = scene.kb.getkey() # get keyboard info 139 | if s == 'a': 140 | #align key pressed - align 141 | yaw_offset += -yaw 142 | 143 | 144 | sub = rospy.Subscriber('xqserial_server/IMU', Imu, processIMU_message) 145 | rospy.spin() 146 | -------------------------------------------------------------------------------- /src/DiffDriverController.cpp: -------------------------------------------------------------------------------- 1 | #include "DiffDriverController.h" 2 | #include 3 | 4 | namespace xqserial_server 5 | { 6 | 7 | DiffDriverController::DiffDriverController() 8 | { 9 | max_wheelspeed = 2.0; 10 | cmd_topic = "cmd_vel"; 11 | xq_status = new StatusPublisher(); 12 | cmd_serial = NULL; 13 | MoveFlag = true; 14 | galileoStatus_.mapStatus = 0; 15 | R_min_ = 0.25; 16 | } 17 | 18 | DiffDriverController::DiffDriverController(double max_speed_, std::string cmd_topic_, StatusPublisher *xq_status_, CallbackAsyncSerial *cmd_serial_,double r_min) 19 | { 20 | MoveFlag = true; 21 | max_wheelspeed = max_speed_; 22 | cmd_topic = cmd_topic_; 23 | xq_status = xq_status_; 24 | cmd_serial = cmd_serial_; 25 | R_min_ = r_min; 26 | } 27 | 28 | void DiffDriverController::run() 29 | { 30 | ros::NodeHandle nodeHandler; 31 | ros::Subscriber sub = nodeHandler.subscribe(cmd_topic, 1, &DiffDriverController::sendcmd, this); 32 | ros::Subscriber sub2 = nodeHandler.subscribe("/imu_cal", 1, &DiffDriverController::imuCalibration, this); 33 | ros::Subscriber sub3 = nodeHandler.subscribe("/global_move_flag", 1, &DiffDriverController::updateMoveFlag, this); 34 | ros::Subscriber sub4 = nodeHandler.subscribe("/barDetectFlag", 1, &DiffDriverController::updateBarDetectFlag, this); 35 | ros::Subscriber sub5 = nodeHandler.subscribe("/galileo/status", 1, &DiffDriverController::UpdateNavStatus, this); 36 | ros::spin(); 37 | } 38 | void DiffDriverController::updateMoveFlag(const std_msgs::Bool &moveFlag) 39 | { 40 | boost::mutex::scoped_lock lock(mMutex); 41 | MoveFlag = moveFlag.data; 42 | } 43 | void DiffDriverController::imuCalibration(const std_msgs::Bool &calFlag) 44 | { 45 | if (calFlag.data) 46 | { 47 | //下发底层ｉｍｕ标定命令 48 | char cmd_str[5] = {(char)0xcd, (char)0xeb, (char)0xd7, (char)0x01, (char)0x43}; 49 | if (NULL != cmd_serial) 50 | { 51 | cmd_serial->write(cmd_str, 5); 52 | } 53 | } 54 | } 55 | void DiffDriverController::updateBarDetectFlag(const std_msgs::Bool &DetectFlag) 56 | { 57 | if (DetectFlag.data) 58 | { 59 | //下发底层红外开启命令 60 | char cmd_str[6] = {(char)0xcd, (char)0xeb, (char)0xd7, (char)0x02, (char)0x44, (char)0x01}; 61 | if (NULL != cmd_serial) 62 | { 63 | cmd_serial->write(cmd_str, 6); 64 | } 65 | } 66 | else 67 | { 68 | //下发底层红外禁用命令 69 | char cmd_str[6] = {(char)0xcd, (char)0xeb, (char)0xd7, (char)0x02, (char)0x44, (char)0x00}; 70 | if (NULL != cmd_serial) 71 | { 72 | cmd_serial->write(cmd_str, 6); 73 | } 74 | } 75 | } 76 | void DiffDriverController::sendcmd(const geometry_msgs::Twist &command) 77 | { 78 | static time_t t1 = time(NULL), t2; 79 | int i = 0, wheel_ppr = 1; 80 | double separation = 0, radius = 0, speed_lin = 0, speed_ang = 0, speed_temp[2]; 81 | char speed[2] = {0, 0}; //右一左二 82 | char cmd_str[13] = {(char)0xcd, (char)0xeb, (char)0xd7, (char)0x09, (char)0x74, (char)0x53, (char)0x53, (char)0x53, (char)0x53, (char)0x00, (char)0x00, (char)0x00, (char)0x00}; 83 | 84 | if (xq_status->get_status() == 0) 85 | return; //底层还在初始化 86 | separation = xq_status->get_wheel_separation(); 87 | radius = xq_status->get_wheel_radius(); 88 | wheel_ppr = xq_status->get_wheel_ppr(); 89 | float x_filter = command.linear.x, z_filter = command.angular.z ; 90 | { 91 | //先过滤速度 92 | boost::mutex::scoped_lock lock(mStausMutex_); 93 | if(galileoStatus_.mapStatus == 1) 94 | { 95 | if(command.angular.z <-0.001 || command.angular.z>0.001 ) 96 | { 97 | float R_now = std::fabs(command.linear.x / command.angular.z); 98 | if(R_now < R_min_) 99 | { 100 | if(command.angular.z>0.001) 101 | { 102 | z_filter = std::fabs(x_filter/R_min_); 103 | } 104 | else 105 | { 106 | z_filter = -std::fabs(x_filter/R_min_); 107 | } 108 | } 109 | } 110 | } 111 | } 112 | 113 | if(std::fabs(x_filter)<0.1) 114 | { 115 | if(z_filter>0.0002&&z_filter<0.15) z_filter=0.15; 116 | if(z_filter<-0.0002&&z_filter>-0.15) z_filter=-0.15; 117 | } 118 | if(x_filter>0 && x_filter<0.05) x_filter=0.05; 119 | if(x_filter<0 && x_filter>-0.05) x_filter=-0.05; 120 | //转换速度单位，由米转换成转 121 | speed_lin = x_filter / (2.0 * PI * radius); 122 | speed_ang = z_filter * separation / (2.0 * PI * radius); 123 | 124 | float scale = std::max(std::abs(speed_lin + speed_ang / 2.0), std::abs(speed_lin - speed_ang / 2.0)) / max_wheelspeed; 125 | if (scale > 1.0) 126 | { 127 | scale = 1.0 / scale; 128 | } 129 | else 130 | { 131 | scale = 1.0; 132 | } 133 | //转出最大速度百分比,并进行限幅 134 | speed_temp[0] = scale * (speed_lin + speed_ang / 2) / max_wheelspeed * 100.0; 135 | speed_temp[0] = std::min(speed_temp[0], 100.0); 136 | speed_temp[0] = std::max(-100.0, speed_temp[0]); 137 | 138 | speed_temp[1] = scale * (speed_lin - speed_ang / 2) / max_wheelspeed * 100.0; 139 | speed_temp[1] = std::min(speed_temp[1], 100.0); 140 | speed_temp[1] = std::max(-100.0, speed_temp[1]); 141 | 142 | //std::cout<<"radius "< 0) 155 | { 156 | cmd_str[5 + i] = (char)0x46; //F 157 | cmd_str[9 + i] = speed[i]; 158 | } 159 | else 160 | { 161 | cmd_str[5 + i] = (char)0x53; //S 162 | cmd_str[9 + i] = (char)0x00; 163 | } 164 | } 165 | 166 | // std::cout<<"distance1 "<car_status.distance1<car_status.distance2<car_status.distance3<get_status()==2) 170 | // { 171 | // //有障碍物 172 | // if(xq_status->car_status.distance1<30&&xq_status->car_status.distance1>0&&cmd_str[6]==(char)0x46) 173 | // { 174 | // cmd_str[6]=(char)0x53; 175 | // } 176 | // if(xq_status->car_status.distance2<30&&xq_status->car_status.distance2>0&&cmd_str[5]==(char)0x46) 177 | // { 178 | // cmd_str[5]=(char)0x53; 179 | // } 180 | // if(xq_status->car_status.distance3<20&&xq_status->car_status.distance3>0&&(cmd_str[5]==(char)0x42||cmd_str[6]==(char)0x42)) 181 | // { 182 | // cmd_str[5]=(char)0x53; 183 | // cmd_str[6]=(char)0x53; 184 | // } 185 | // if(xq_status->car_status.distance1<15&&xq_status->car_status.distance1>0&&(cmd_str[5]==(char)0x46||cmd_str[6]==(char)0x46)) 186 | // { 187 | // cmd_str[5]=(char)0x53; 188 | // cmd_str[6]=(char)0x53; 189 | // } 190 | // if(xq_status->car_status.distance2<15&&xq_status->car_status.distance2>0&&(cmd_str[5]==(char)0x46||cmd_str[6]==(char)0x46)) 191 | // { 192 | // cmd_str[5]=(char)0x53; 193 | // cmd_str[6]=(char)0x53; 194 | // } 195 | // } 196 | 197 | boost::mutex::scoped_lock lock(mMutex); 198 | if (!MoveFlag) 199 | { 200 | cmd_str[5] = (char)0x53; 201 | cmd_str[6] = (char)0x53; 202 | } 203 | if (NULL != cmd_serial) 204 | { 205 | cmd_serial->write(cmd_str, 13); 206 | } 207 | 208 | // command.linear.x 209 | } 210 | 211 | void DiffDriverController::UpdateNavStatus(const galileo_serial_server::GalileoStatus& current_receive_status) 212 | { 213 | boost::mutex::scoped_lock lock(mStausMutex_); 214 | galileoStatus_.navStatus = current_receive_status.navStatus; 215 | galileoStatus_.visualStatus = current_receive_status.visualStatus; 216 | galileoStatus_.chargeStatus = current_receive_status.chargeStatus; 217 | galileoStatus_.mapStatus = current_receive_status.mapStatus; 218 | 219 | } 220 | 221 | } // namespace xqserial_server 222 | -------------------------------------------------------------------------------- /include/AsyncSerial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: AsyncSerial.h 3 | * Author: Terraneo Federico 4 | * Distributed under the Boost Software License, Version 1.0. 5 | * Created on September 7, 2009, 10:46 AM 6 | */ 7 | 8 | #ifndef ASYNCSERIAL_H 9 | #define ASYNCSERIAL_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | /** 20 | * Used internally (pimpl) 21 | */ 22 | class AsyncSerialImpl; 23 | 24 | /** 25 | * Asyncronous serial class. 26 | * Intended to be a base class. 27 | */ 28 | class AsyncSerial : private boost::noncopyable 29 | { 30 | public: 31 | AsyncSerial(); 32 | 33 | /** 34 | * Constructor. Creates and opens a serial device. 35 | * \param devname serial device name, example "/dev/ttyS0" or "COM1" 36 | * \param baud_rate serial baud rate 37 | * \param opt_parity serial parity, default none 38 | * \param opt_csize serial character size, default 8bit 39 | * \param opt_flow serial flow control, default none 40 | * \param opt_stop serial stop bits, default 1 41 | * \throws boost::system::system_error if cannot open the 42 | * serial device 43 | */ 44 | AsyncSerial(const std::string &devname, unsigned int baud_rate, 45 | boost::asio::serial_port_base::parity opt_parity = 46 | boost::asio::serial_port_base::parity( 47 | boost::asio::serial_port_base::parity::none), 48 | boost::asio::serial_port_base::character_size opt_csize = 49 | boost::asio::serial_port_base::character_size(8), 50 | boost::asio::serial_port_base::flow_control opt_flow = 51 | boost::asio::serial_port_base::flow_control( 52 | boost::asio::serial_port_base::flow_control::none), 53 | boost::asio::serial_port_base::stop_bits opt_stop = 54 | boost::asio::serial_port_base::stop_bits( 55 | boost::asio::serial_port_base::stop_bits::one)); 56 | 57 | /** 58 | * Opens a serial device. 59 | * \param devname serial device name, example "/dev/ttyS0" or "COM1" 60 | * \param baud_rate serial baud rate 61 | * \param opt_parity serial parity, default none 62 | * \param opt_csize serial character size, default 8bit 63 | * \param opt_flow serial flow control, default none 64 | * \param opt_stop serial stop bits, default 1 65 | * \throws boost::system::system_error if cannot open the 66 | * serial device 67 | */ 68 | void open(const std::string &devname, unsigned int baud_rate, 69 | boost::asio::serial_port_base::parity opt_parity = 70 | boost::asio::serial_port_base::parity( 71 | boost::asio::serial_port_base::parity::none), 72 | boost::asio::serial_port_base::character_size opt_csize = 73 | boost::asio::serial_port_base::character_size(8), 74 | boost::asio::serial_port_base::flow_control opt_flow = 75 | boost::asio::serial_port_base::flow_control( 76 | boost::asio::serial_port_base::flow_control::none), 77 | boost::asio::serial_port_base::stop_bits opt_stop = 78 | boost::asio::serial_port_base::stop_bits( 79 | boost::asio::serial_port_base::stop_bits::one)); 80 | 81 | /** 82 | * \return true if serial device is open 83 | */ 84 | bool isOpen() const; 85 | 86 | /** 87 | * \return true if error were found 88 | */ 89 | bool errorStatus() const; 90 | 91 | /** 92 | * Close the serial device 93 | * \throws boost::system::system_error if any error 94 | */ 95 | void close(); 96 | 97 | /** 98 | * Write data asynchronously. Returns immediately. 99 | * \param data array of char to be sent through the serial device 100 | * \param size array size 101 | */ 102 | void write(const char *data, size_t size); 103 | 104 | /** 105 | * Write data asynchronously. Returns immediately. 106 | * \param data to be sent through the serial device 107 | */ 108 | void write(const std::vector &data); 109 | 110 | /** 111 | * Write a string asynchronously. Returns immediately. 112 | * Can be used to send ASCII data to the serial device. 113 | * To send binary data, use write() 114 | * \param s string to send 115 | */ 116 | void writeString(const std::string &s); 117 | 118 | virtual ~AsyncSerial() = 0; 119 | 120 | /** 121 | * Read buffer maximum size 122 | */ 123 | static const int readBufferSize = 1024; 124 | 125 | private: 126 | /** 127 | * Callback called to start an asynchronous read operation. 128 | * This callback is called by the io_service in the spawned thread. 129 | */ 130 | void doRead(); 131 | 132 | /** 133 | * Callback called at the end of the asynchronous operation. 134 | * This callback is called by the io_service in the spawned thread. 135 | */ 136 | void readEnd(const boost::system::error_code &error, 137 | size_t bytes_transferred); 138 | 139 | /** 140 | * Callback called to start an asynchronous write operation. 141 | * If it is already in progress, does nothing. 142 | * This callback is called by the io_service in the spawned thread. 143 | */ 144 | void doWrite(); 145 | 146 | /** 147 | * Callback called at the end of an asynchronuous write operation, 148 | * if there is more data to write, restarts a new write operation. 149 | * This callback is called by the io_service in the spawned thread. 150 | */ 151 | void writeEnd(const boost::system::error_code &error); 152 | 153 | /** 154 | * Callback to close serial port 155 | */ 156 | void doClose(); 157 | 158 | boost::shared_ptr pimpl; 159 | 160 | protected: 161 | /** 162 | * To allow derived classes to report errors 163 | * \param e error status 164 | */ 165 | void setErrorStatus(bool e); 166 | 167 | /** 168 | * To allow derived classes to set a read callback 169 | */ 170 | void setReadCallback(const boost::function &callback); 171 | 172 | /** 173 | * To unregister the read callback in the derived class destructor so it 174 | * does not get called after the derived class destructor but before the 175 | * base class destructor 176 | */ 177 | void clearReadCallback(); 178 | }; 179 | 180 | /** 181 | * Asynchronous serial class with read callback. User code can write data 182 | * from one thread, and read data will be reported through a callback called 183 | * from a separate thred. 184 | */ 185 | class CallbackAsyncSerial : public AsyncSerial 186 | { 187 | public: 188 | CallbackAsyncSerial(); 189 | 190 | /** 191 | * Opens a serial device. 192 | * \param devname serial device name, example "/dev/ttyS0" or "COM1" 193 | * \param baud_rate serial baud rate 194 | * \param opt_parity serial parity, default none 195 | * \param opt_csize serial character size, default 8bit 196 | * \param opt_flow serial flow control, default none 197 | * \param opt_stop serial stop bits, default 1 198 | * \throws boost::system::system_error if cannot open the 199 | * serial device 200 | */ 201 | CallbackAsyncSerial(const std::string &devname, unsigned int baud_rate, 202 | boost::asio::serial_port_base::parity opt_parity = 203 | boost::asio::serial_port_base::parity( 204 | boost::asio::serial_port_base::parity::none), 205 | boost::asio::serial_port_base::character_size opt_csize = 206 | boost::asio::serial_port_base::character_size(8), 207 | boost::asio::serial_port_base::flow_control opt_flow = 208 | boost::asio::serial_port_base::flow_control( 209 | boost::asio::serial_port_base::flow_control::none), 210 | boost::asio::serial_port_base::stop_bits opt_stop = 211 | boost::asio::serial_port_base::stop_bits( 212 | boost::asio::serial_port_base::stop_bits::one)); 213 | 214 | /** 215 | * Set the read callback, the callback will be called from a thread 216 | * owned by the CallbackAsyncSerial class when data arrives from the 217 | * serial port. 218 | * \param callback the receive callback 219 | */ 220 | void setCallback(const boost::function &callback); 221 | 222 | /** 223 | * Removes the callback. Any data received after this function call will 224 | * be lost. 225 | */ 226 | void clearCallback(); 227 | 228 | virtual ~CallbackAsyncSerial(); 229 | }; 230 | 231 | #endif //ASYNCSERIAL_H 232 | -------------------------------------------------------------------------------- /CMakeLists.txt.user: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | ProjectExplorer.Project.ActiveTarget 7 | 0 8 | 9 | 10 | ProjectExplorer.Project.EditorSettings 11 | 12 | true 13 | false 14 | true 15 | 16 | Cpp 17 | 18 | CppGlobal 19 | 20 | 21 | 22 | QmlJS 23 | 24 | QmlJSGlobal 25 | 26 | 27 | 2 28 | UTF-8 29 | false 30 | 4 31 | false 32 | true 33 | 1 34 | true 35 | 0 36 | true 37 | 0 38 | 8 39 | true 40 | 1 41 | true 42 | true 43 | true 44 | false 45 | 46 | 47 | 48 | ProjectExplorer.Project.PluginSettings 49 | 50 | 51 | 52 | ProjectExplorer.Project.Target.0 53 | 54 | Desktop 55 | Desktop 56 | {6b023c59-f11b-4247-947a-7c6d24c2db52} 57 | 0 58 | 0 59 | 0 60 | 61 | false 62 | -DCMAKE_BUILD_TYPE=Debug 63 | /home/frank/catkin_ws/src/xqserial_server/build 64 | 65 | 66 | 67 | 68 | false 69 | false 70 | true 71 | Make 72 | 73 | CMakeProjectManager.MakeStep 74 | 75 | 1 76 | Build 77 | 78 | ProjectExplorer.BuildSteps.Build 79 | 80 | 81 | 82 | clean 83 | 84 | true 85 | false 86 | true 87 | Make 88 | 89 | CMakeProjectManager.MakeStep 90 | 91 | 1 92 | Clean 93 | 94 | ProjectExplorer.BuildSteps.Clean 95 | 96 | 2 97 | false 98 | 99 | Default 100 | Default 101 | CMakeProjectManager.CMakeBuildConfiguration 102 | 103 | 1 104 | 105 | 106 | 0 107 | Deploy 108 | 109 | ProjectExplorer.BuildSteps.Deploy 110 | 111 | 1 112 | Deploy locally 113 | 114 | ProjectExplorer.DefaultDeployConfiguration 115 | 116 | 1 117 | 118 | 119 | 120 | false 121 | false 122 | false 123 | false 124 | true 125 | 0.01 126 | 10 127 | true 128 | 1 129 | 25 130 | 131 | 1 132 | true 133 | false 134 | true 135 | valgrind 136 | 137 | 0 138 | 1 139 | 2 140 | 3 141 | 4 142 | 5 143 | 6 144 | 7 145 | 8 146 | 9 147 | 10 148 | 11 149 | 12 150 | 13 151 | 14 152 | 153 | xqserial_server 154 | _port:=/dev/ttyUSB0 _baud:=9600 155 | true 156 | 157 | 2 158 | 159 | xqserial_server 160 | 161 | CMakeProjectManager.CMakeRunConfiguration.xqserial_server 162 | 3768 163 | true 164 | false 165 | false 166 | false 167 | true 168 | 169 | 1 170 | 171 | 172 | 173 | ProjectExplorer.Project.TargetCount 174 | 1 175 | 176 | 177 | ProjectExplorer.Project.Updater.EnvironmentId 178 | {48bf2ed2-0237-461f-af7c-2158884db25a} 179 | 180 | 181 | ProjectExplorer.Project.Updater.FileVersion 182 | 15 183 | 184 | 185 | -------------------------------------------------------------------------------- /src/StatusPublisher.cpp: -------------------------------------------------------------------------------- 1 | #include "StatusPublisher.h" 2 | #include "AsyncSerial.h" 3 | #include 4 | #include 5 | #include 6 | 7 | #define DISABLE 0 8 | #define ENABLE 1 9 | 10 | namespace xqserial_server 11 | { 12 | typedef sensor_msgs::PointCloud2 PointCloud; 13 | 14 | StatusPublisher::StatusPublisher() 15 | { 16 | mbUpdated = false; 17 | wheel_separation = 0.37; 18 | wheel_radius = 0.06; 19 | 20 | CarPos2D.x = 0.0; 21 | CarPos2D.y = 0.0; 22 | CarPos2D.theta = 0.0; 23 | 24 | CarTwist.linear.x = 0.0; 25 | CarTwist.linear.y = 0.0; 26 | CarTwist.linear.z = 0.0; 27 | CarTwist.angular.x = 0.0; 28 | CarTwist.angular.y = 0.0; 29 | CarTwist.angular.z = 0.0; 30 | 31 | CarPower.data = 0.0; 32 | 33 | int i = 0; 34 | int *status; 35 | status = (int *)&car_status; 36 | for (i = 0; i < 23; i++) 37 | { 38 | status[i] = 0; 39 | } 40 | car_status.encoder_ppr = 4 * 12 * 64; 41 | 42 | mPose2DPub = mNH.advertise("xqserial_server/Pose2D", 1, true); 43 | mStatusFlagPub = mNH.advertise("xqserial_server/StatusFlag", 1, true); 44 | mTwistPub = mNH.advertise("xqserial_server/Twist", 1, true); 45 | mPowerPub = mNH.advertise("xqserial_server/Power", 1, true); 46 | mOdomPub = mNH.advertise("xqserial_server/Odom", 1, true); 47 | pub_barpoint_cloud_ = mNH.advertise("kinect/barpoints", 1, true); 48 | pub_clearpoint_cloud_ = mNH.advertise("kinect/clearpoints", 1, true); 49 | mIMUPub = mNH.advertise("xqserial_server/IMU", 1, true); 50 | /* static tf::TransformBroadcaster br; 51 | tf::Quaternion q; 52 | tf::Transform transform; 53 | transform.setOrigin( tf::Vector3(0.0, 0.0, 0.13) );//摄像头距离地面高度13cm 54 | q.setRPY(0, 0, 0); 55 | transform.setRotation(q); 56 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_footprint", "base_link")); 57 | */ 58 | base_time_ = ros::Time::now().toSec(); 59 | } 60 | 61 | StatusPublisher::StatusPublisher(double separation, double radius) 62 | { 63 | new (this) StatusPublisher(); 64 | wheel_separation = separation; 65 | wheel_radius = radius; 66 | } 67 | 68 | void StatusPublisher::Update(const char data[], unsigned int len) 69 | { 70 | // if(len <1) return; 71 | // static char data2[1024]; 72 | // static int len2=0; 73 | boost::mutex::scoped_lock lock(mMutex); 74 | 75 | int i = 0, j = 0; 76 | int *receive_byte; 77 | static unsigned char last_str[2] = {0x00, 0x00}; 78 | static unsigned char new_packed_ctr = DISABLE; //ENABLE表示新包开始，DISABLE 表示上一个包还未处理完； 79 | static int new_packed_ok_len = 0; //包的理论长度 80 | static int new_packed_len = 0; //包的实际长度 81 | static unsigned char cmd_string_buf[512]; 82 | unsigned char current_str = 0x00; 83 | const int cmd_string_max_size = 512; 84 | receive_byte = (int *)&car_status; 85 | //int ii=0; 86 | //boost::mutex::scoped_lock lock(mMutex); 87 | 88 | // if(len<119) 89 | // { 90 | // std::cout<<"len0:"< cmd_string_max_size) 118 | new_packed_ok_len = cmd_string_max_size; //包内容最大长度有限制 119 | new_packed_ctr = DISABLE; 120 | //std::cout<<"runup2 "<< new_packed_len<< new_packed_ok_len< 0) 137 | { 138 | // std::cout<<"runup4 "<3000||car_status.encoder_delta_car<-3000)) 180 | // { 181 | // std::cout<<"len:"<< len < 0.05 || delta_car < -0.05) 249 | { 250 | // std::cout<<"get you!"<3000||car_status.encoder_delta_car<-3000) 254 | // { 255 | // std::cout<<"delta_encoder_car:"<< car_status.encoder_delta_car < 270) 267 | delta_theta -= 360; 268 | if (delta_theta < -270) 269 | delta_theta += 360; 270 | 271 | if ((!theta_updateflag) || delta_theta > 20 || delta_theta < -20) 272 | { 273 | delta_theta = 0; 274 | } 275 | CarPos2D.x += delta_x; 276 | CarPos2D.y += delta_y; 277 | CarPos2D.theta += delta_theta; 278 | 279 | if (CarPos2D.theta > 360.0) 280 | CarPos2D.theta -= 360; 281 | if (CarPos2D.theta < 0.0) 282 | CarPos2D.theta += 360; 283 | 284 | mPose2DPub.publish(CarPos2D); 285 | 286 | //flag 287 | std_msgs::Int32 flag; 288 | flag.data = car_status.status; 289 | //底层障碍物信息 290 | if ((car_status.distance1 + car_status.distance2 + car_status.distance3 + car_status.distance4) > 0.1 && (car_status.distance1 + car_status.distance2 + car_status.distance3 + car_status.distance4) < 5.0) 291 | { 292 | //有障碍物 293 | flag.data = 2; 294 | } 295 | mStatusFlagPub.publish(flag); 296 | 297 | int barArea_nums = 0; 298 | int clearArea_nums = 0; 299 | if (car_status.distance1 > 0.1) 300 | { 301 | barArea_nums += 3; 302 | } 303 | else 304 | { 305 | clearArea_nums += 6; 306 | } 307 | if (car_status.distance2 > 0.1) 308 | { 309 | barArea_nums += 3; 310 | } 311 | else 312 | { 313 | clearArea_nums += 6; 314 | } 315 | if (car_status.distance4 > 0.1) 316 | { 317 | barArea_nums += 3; 318 | } 319 | else 320 | { 321 | clearArea_nums += 6; 322 | } 323 | 324 | if (barArea_nums > 0) 325 | { 326 | //发布雷区 327 | PointCloud::Ptr barcloud_msg(new PointCloud); 328 | barcloud_msg->header.stamp = current_time.fromSec(base_time_); 329 | barcloud_msg->height = 1; 330 | barcloud_msg->width = barArea_nums; 331 | barcloud_msg->is_dense = true; 332 | barcloud_msg->is_bigendian = false; 333 | barcloud_msg->header.frame_id = "kinect_link_new"; 334 | sensor_msgs::PointCloud2Modifier pcd_modifier1(*barcloud_msg); 335 | pcd_modifier1.setPointCloud2FieldsByString(1, "xyz"); 336 | sensor_msgs::PointCloud2Iterator bariter_x(*barcloud_msg, "x"); 337 | sensor_msgs::PointCloud2Iterator bariter_y(*barcloud_msg, "y"); 338 | sensor_msgs::PointCloud2Iterator bariter_z(*barcloud_msg, "z"); 339 | if (car_status.distance2 > 0.1) 340 | { 341 | for (int k = 0; k < 3; k++, ++bariter_x, ++bariter_y, ++bariter_z) 342 | { 343 | *bariter_x = 0.2; 344 | *bariter_y = -0.10 - k * 0.05; 345 | *bariter_z = 0.15; 346 | } 347 | } 348 | if (car_status.distance4 > 0.1) 349 | { 350 | for (int k = 0; k < 3; k++, ++bariter_x, ++bariter_y, ++bariter_z) 351 | { 352 | *bariter_x = 0.2; 353 | *bariter_y = -0.1 + k * 0.05; 354 | *bariter_z = 0.15; 355 | } 356 | } 357 | if (car_status.distance1 > 0.1) 358 | { 359 | for (int k = 0; k < 3; k++, ++bariter_x, ++bariter_y, ++bariter_z) 360 | { 361 | *bariter_x = 0.2; 362 | *bariter_y = 0.05 + k * 0.05; 363 | *bariter_z = 0.15; 364 | } 365 | } 366 | if (ii % 5 == 0) 367 | { 368 | pub_barpoint_cloud_.publish(barcloud_msg); 369 | } 370 | } 371 | if (clearArea_nums > 0) 372 | { 373 | //发布雷区 374 | PointCloud::Ptr clearcloud_msg(new PointCloud); 375 | clearcloud_msg->header.stamp = current_time.fromSec(base_time_); 376 | clearcloud_msg->height = 1; 377 | clearcloud_msg->width = clearArea_nums; 378 | clearcloud_msg->is_dense = true; 379 | clearcloud_msg->is_bigendian = false; 380 | clearcloud_msg->header.frame_id = "kinect_link_new"; 381 | sensor_msgs::PointCloud2Modifier pcd_modifier1(*clearcloud_msg); 382 | pcd_modifier1.setPointCloud2FieldsByString(1, "xyz"); 383 | sensor_msgs::PointCloud2Iterator cleariter_x(*clearcloud_msg, "x"); 384 | sensor_msgs::PointCloud2Iterator cleariter_y(*clearcloud_msg, "y"); 385 | sensor_msgs::PointCloud2Iterator cleariter_z(*clearcloud_msg, "z"); 386 | if (car_status.distance2 < 0.1) 387 | { 388 | for (int k = 0; k < 3; k++, ++cleariter_x, ++cleariter_y, ++cleariter_z) 389 | { 390 | *cleariter_x = 0.2; 391 | *cleariter_y = -0.1 - k * 0.05; 392 | *cleariter_z = 0.0; 393 | } 394 | for (int k = 0; k < 3; k++, ++cleariter_x, ++cleariter_y, ++cleariter_z) 395 | { 396 | *cleariter_x = 0.15; 397 | *cleariter_y = -0.1 - k * 0.05; 398 | *cleariter_z = 0.0; 399 | } 400 | } 401 | if (car_status.distance4 < 0.1) 402 | { 403 | for (int k = 0; k < 3; k++, ++cleariter_x, ++cleariter_y, ++cleariter_z) 404 | { 405 | *cleariter_x = 0.2; 406 | *cleariter_y = -0.1 + k * 0.05; 407 | *cleariter_z = 0.0; 408 | } 409 | for (int k = 0; k < 3; k++, ++cleariter_x, ++cleariter_y, ++cleariter_z) 410 | { 411 | *cleariter_x = 0.15; 412 | *cleariter_y = -0.1 + k * 0.05; 413 | *cleariter_z = 0.0; 414 | } 415 | } 416 | if (car_status.distance1 < 0.1) 417 | { 418 | for (int k = 0; k < 3; k++, ++cleariter_x, ++cleariter_y, ++cleariter_z) 419 | { 420 | *cleariter_x = 0.2; 421 | *cleariter_y = 0.05 + k * 0.05; 422 | *cleariter_z = 0.0; 423 | } 424 | for (int k = 0; k < 3; k++, ++cleariter_x, ++cleariter_y, ++cleariter_z) 425 | { 426 | *cleariter_x = 0.15; 427 | *cleariter_y = 0.05 + k * 0.05; 428 | *cleariter_z = 0.0; 429 | } 430 | } 431 | if (ii % 5 == 0) 432 | { 433 | pub_clearpoint_cloud_.publish(clearcloud_msg); 434 | } 435 | } 436 | 437 | //Twist 438 | double angle_speed; 439 | CarTwist.linear.x = delta_car * 50.0f; 440 | angle_speed = -car_status.IMU[5]; 441 | CarTwist.angular.z = angle_speed * PI / 180.0f; 442 | mTwistPub.publish(CarTwist); 443 | 444 | CarPower.data = car_status.power; 445 | mPowerPub.publish(CarPower); 446 | 447 | CarOdom.header.stamp = current_time.fromSec(base_time_); 448 | CarOdom.header.frame_id = "odom"; 449 | CarOdom.pose.pose.position.x = CarPos2D.x; 450 | CarOdom.pose.pose.position.y = CarPos2D.y; 451 | CarOdom.pose.pose.position.z = 0.0f; 452 | geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(CarPos2D.theta / 180.0f * PI); 453 | CarOdom.pose.pose.orientation = odom_quat; 454 | CarOdom.pose.covariance = boost::assign::list_of(var_len)(0)(0)(0)(0)(0)(0)(var_len)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(var_angle); 455 | CarOdom.child_frame_id = "base_footprint"; 456 | CarOdom.twist.twist.linear.x = CarTwist.linear.x; // * cos(CarPos2D.theta* PI / 180.0f); 457 | CarOdom.twist.twist.linear.y = CarTwist.linear.y; // * sin(CarPos2D.theta* PI / 180.0f); 458 | CarOdom.twist.twist.angular.z = CarTwist.angular.z; 459 | CarOdom.twist.covariance = boost::assign::list_of(var_len)(0)(0)(0)(0)(0)(0)(var_len)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(var_angle); 460 | mOdomPub.publish(CarOdom); 461 | 462 | //publish IMU 463 | tf::Quaternion q_imu; 464 | q_imu.setRPY(0, 0, car_status.theta / 180.0 * PI); 465 | CarIMU.header.stamp = current_time; 466 | CarIMU.header.frame_id = "imu"; 467 | CarIMU.orientation.x = q_imu.x(); 468 | CarIMU.orientation.y = q_imu.y(); 469 | CarIMU.orientation.z = q_imu.z(); 470 | CarIMU.orientation.w = q_imu.w(); 471 | 472 | CarIMU.angular_velocity.x = -car_status.IMU[3] * PI / 180.0f; 473 | CarIMU.angular_velocity.y = car_status.IMU[4] * PI / 180.0f; 474 | CarIMU.angular_velocity.z = -car_status.IMU[5] * PI / 180.0f; 475 | CarIMU.linear_acceleration.x = -car_status.IMU[0]; 476 | CarIMU.linear_acceleration.y = car_status.IMU[1]; 477 | CarIMU.linear_acceleration.z = -car_status.IMU[2]; 478 | 479 | mIMUPub.publish(CarIMU); 480 | 481 | // pub transform 482 | 483 | static tf::TransformBroadcaster br; 484 | tf::Quaternion q; 485 | tf::Transform transform; 486 | transform.setOrigin(tf::Vector3(CarPos2D.x, CarPos2D.y, 0.0)); 487 | q.setRPY(0, 0, CarPos2D.theta / 180 * PI); 488 | transform.setRotation(q); 489 | br.sendTransform(tf::StampedTransform(transform, current_time.fromSec(base_time_), "odom", "base_footprint")); 490 | 491 | ros::spinOnce(); 492 | 493 | mbUpdated = false; 494 | } 495 | } 496 | 497 | double StatusPublisher::get_wheel_separation() 498 | { 499 | return wheel_separation; 500 | } 501 | 502 | double StatusPublisher::get_wheel_radius() 503 | { 504 | return wheel_radius; 505 | } 506 | 507 | int StatusPublisher::get_wheel_ppr() 508 | { 509 | return car_status.encoder_ppr; 510 | } 511 | 512 | void StatusPublisher::get_wheel_speed(double speed[2]) 513 | { 514 | //右一左二 515 | speed[0] = car_status.omga_r / car_status.encoder_ppr * 2.0 * PI * wheel_radius; 516 | speed[1] = car_status.omga_l / car_status.encoder_ppr * 2.0 * PI * wheel_radius; 517 | } 518 | 519 | geometry_msgs::Pose2D StatusPublisher::get_CarPos2D() 520 | { 521 | return CarPos2D; 522 | } 523 | 524 | geometry_msgs::Twist StatusPublisher::get_CarTwist() 525 | { 526 | return CarTwist; 527 | } 528 | 529 | std_msgs::Float64 StatusPublisher::get_power() 530 | { 531 | return CarPower; 532 | } 533 | 534 | nav_msgs::Odometry StatusPublisher::get_odom() 535 | { 536 | return CarOdom; 537 | } 538 | int StatusPublisher::get_status() 539 | { 540 | return car_status.status; 541 | } 542 | 543 | } //namespace xqserial_server 544 | -------------------------------------------------------------------------------- /src/AsyncSerial.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: AsyncSerial.cpp 3 | * Author: Terraneo Federico 4 | * Distributed under the Boost Software License, Version 1.0. 5 | * Created on September 7, 2009, 10:46 AM 6 | * 7 | * v1.02: Fixed a bug in BufferedAsyncSerial: Using the default constructor 8 | * the callback was not set up and reading didn't work. 9 | * 10 | * v1.01: Fixed a bug that did not allow to reopen a closed serial port. 11 | * 12 | * v1.00: First release. 13 | * 14 | * IMPORTANT: 15 | * On Mac OS X boost asio's serial ports have bugs, and the usual implementation 16 | * of this class does not work. So a workaround class was written temporarily, 17 | * until asio (hopefully) will fix Mac compatibility for serial ports. 18 | * 19 | * Please note that unlike said in the documentation on OS X until asio will 20 | * be fixed serial port *writes* are *not* asynchronous, but at least 21 | * asynchronous *read* works. 22 | * In addition the serial port open ignores the following options: parity, 23 | * character size, flow, stop bits, and defaults to 8N1 format. 24 | * I know it is bad but at least it's better than nothing. 25 | * 26 | */ 27 | 28 | #include "AsyncSerial.h" 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | using namespace std; 38 | using namespace boost; 39 | 40 | // 41 | //Class AsyncSerial 42 | // 43 | 44 | #ifndef __APPLE__ 45 | 46 | class AsyncSerialImpl : private boost::noncopyable 47 | { 48 | public: 49 | AsyncSerialImpl() : io(), port(io), backgroundThread(), open(false), 50 | error(false) {} 51 | 52 | boost::asio::io_service io; ///< Io service object 53 | boost::asio::serial_port port; ///< Serial port object 54 | boost::thread backgroundThread; ///< Thread that runs read/write operations 55 | bool open; ///< True if port open 56 | bool error; ///< Error flag 57 | mutable boost::mutex errorMutex; ///< Mutex for access to error 58 | 59 | /// Data are queued here before they go in writeBuffer 60 | std::vector writeQueue; 61 | boost::shared_array writeBuffer; ///< Data being written 62 | size_t writeBufferSize; ///< Size of writeBuffer 63 | boost::mutex writeQueueMutex; ///< Mutex for access to writeQueue 64 | char readBuffer[AsyncSerial::readBufferSize]; ///< data being read 65 | char readBuffer1[AsyncSerial::readBufferSize]; 66 | char readBuffer2[AsyncSerial::readBufferSize]; 67 | /// Read complete callback 68 | boost::function callback; 69 | }; 70 | 71 | AsyncSerial::AsyncSerial() : pimpl(new AsyncSerialImpl) 72 | { 73 | } 74 | 75 | AsyncSerial::AsyncSerial(const std::string &devname, unsigned int baud_rate, 76 | asio::serial_port_base::parity opt_parity, 77 | asio::serial_port_base::character_size opt_csize, 78 | asio::serial_port_base::flow_control opt_flow, 79 | asio::serial_port_base::stop_bits opt_stop) 80 | : pimpl(new AsyncSerialImpl) 81 | { 82 | open(devname, baud_rate, opt_parity, opt_csize, opt_flow, opt_stop); 83 | } 84 | 85 | void AsyncSerial::open(const std::string &devname, unsigned int baud_rate, 86 | asio::serial_port_base::parity opt_parity, 87 | asio::serial_port_base::character_size opt_csize, 88 | asio::serial_port_base::flow_control opt_flow, 89 | asio::serial_port_base::stop_bits opt_stop) 90 | { 91 | if (isOpen()) 92 | close(); 93 | 94 | setErrorStatus(true); //If an exception is thrown, error_ remains true 95 | pimpl->port.open(devname); 96 | 97 | pimpl->port.set_option(asio::serial_port_base::baud_rate(baud_rate)); 98 | pimpl->port.set_option(opt_parity); 99 | pimpl->port.set_option(opt_csize); 100 | pimpl->port.set_option(opt_flow); 101 | pimpl->port.set_option(opt_stop); 102 | 103 | boost::asio::basic_serial_port::native_type native = pimpl->port.native(); // serial_port_ is the boost's serial port class. 104 | struct serial_struct serial_tempf; 105 | ioctl(native, TIOCGSERIAL, &serial_tempf); 106 | serial_tempf.flags |= ASYNC_LOW_LATENCY; // (0x2000) 107 | ioctl(native, TIOCSSERIAL, &serial_tempf); 108 | 109 | //This gives some work to the io_service before it is started 110 | pimpl->io.post(boost::bind(&AsyncSerial::doRead, this)); 111 | 112 | thread t(boost::bind(&asio::io_service::run, &pimpl->io)); 113 | pimpl->backgroundThread.swap(t); 114 | setErrorStatus(false); //If we get here, no error 115 | pimpl->open = true; //Port is now open 116 | } 117 | 118 | bool AsyncSerial::isOpen() const 119 | { 120 | return pimpl->open; 121 | } 122 | 123 | bool AsyncSerial::errorStatus() const 124 | { 125 | lock_guard l(pimpl->errorMutex); 126 | return pimpl->error; 127 | } 128 | 129 | void AsyncSerial::close() 130 | { 131 | if (!isOpen()) 132 | return; 133 | 134 | pimpl->open = false; 135 | pimpl->io.post(boost::bind(&AsyncSerial::doClose, this)); 136 | pimpl->backgroundThread.join(); 137 | pimpl->io.reset(); 138 | if (errorStatus()) 139 | { 140 | throw(boost::system::system_error(boost::system::error_code(), 141 | "Error while closing the device")); 142 | } 143 | } 144 | 145 | void AsyncSerial::write(const char *data, size_t size) 146 | { 147 | { 148 | lock_guard l(pimpl->writeQueueMutex); 149 | pimpl->writeQueue.insert(pimpl->writeQueue.end(), data, data + size); 150 | } 151 | pimpl->io.post(boost::bind(&AsyncSerial::doWrite, this)); 152 | } 153 | 154 | void AsyncSerial::write(const std::vector &data) 155 | { 156 | { 157 | lock_guard l(pimpl->writeQueueMutex); 158 | pimpl->writeQueue.insert(pimpl->writeQueue.end(), data.begin(), 159 | data.end()); 160 | } 161 | pimpl->io.post(boost::bind(&AsyncSerial::doWrite, this)); 162 | } 163 | 164 | void AsyncSerial::writeString(const std::string &s) 165 | { 166 | { 167 | lock_guard l(pimpl->writeQueueMutex); 168 | pimpl->writeQueue.insert(pimpl->writeQueue.end(), s.begin(), s.end()); 169 | } 170 | pimpl->io.post(boost::bind(&AsyncSerial::doWrite, this)); 171 | } 172 | 173 | AsyncSerial::~AsyncSerial() 174 | { 175 | if (isOpen()) 176 | { 177 | try 178 | { 179 | close(); 180 | } 181 | catch (...) 182 | { 183 | //Don't throw from a destructor 184 | } 185 | } 186 | } 187 | 188 | void AsyncSerial::doRead() 189 | { 190 | pimpl->port.async_read_some(asio::buffer(pimpl->readBuffer, readBufferSize), 191 | boost::bind(&AsyncSerial::readEnd, 192 | this, 193 | asio::placeholders::error, 194 | asio::placeholders::bytes_transferred)); 195 | 196 | // asio::async_read(pimpl->port,asio::buffer(pimpl->readBuffer,readBufferSize),asio::transfer_exactly(54), 197 | // boost::bind(&AsyncSerial::readEnd, 198 | // this, 199 | // asio::placeholders::error, 200 | // asio::placeholders::bytes_transferred)); 201 | } 202 | 203 | void AsyncSerial::readEnd(const boost::system::error_code &error, 204 | size_t bytes_transferred) 205 | { 206 | if (error) 207 | { 208 | #ifdef __APPLE__ 209 | if (error.value() == 45) 210 | { 211 | //Bug on OS X, it might be necessary to repeat the setup 212 | //http://osdir.com/ml/lib.boost.asio.user/2008-08/msg00004.html 213 | doRead(); 214 | return; 215 | } 216 | #endif //__APPLE__ 217 | //error can be true even because the serial port was closed. 218 | //In this case it is not a real error, so ignore 219 | if (isOpen()) 220 | { 221 | doClose(); 222 | setErrorStatus(true); 223 | } 224 | } 225 | else 226 | { 227 | if (pimpl->callback) 228 | { 229 | pimpl->callback(pimpl->readBuffer, bytes_transferred); 230 | } 231 | doRead(); 232 | } 233 | } 234 | 235 | void AsyncSerial::doWrite() 236 | { 237 | //If a write operation is already in progress, do nothing 238 | if (pimpl->writeBuffer == 0) 239 | { 240 | lock_guard l(pimpl->writeQueueMutex); 241 | pimpl->writeBufferSize = pimpl->writeQueue.size(); 242 | pimpl->writeBuffer.reset(new char[pimpl->writeQueue.size()]); 243 | copy(pimpl->writeQueue.begin(), pimpl->writeQueue.end(), 244 | pimpl->writeBuffer.get()); 245 | pimpl->writeQueue.clear(); 246 | async_write(pimpl->port, asio::buffer(pimpl->writeBuffer.get(), pimpl->writeBufferSize), 247 | boost::bind(&AsyncSerial::writeEnd, this, asio::placeholders::error)); 248 | } 249 | } 250 | 251 | void AsyncSerial::writeEnd(const boost::system::error_code &error) 252 | { 253 | if (!error) 254 | { 255 | lock_guard l(pimpl->writeQueueMutex); 256 | if (pimpl->writeQueue.empty()) 257 | { 258 | pimpl->writeBuffer.reset(); 259 | pimpl->writeBufferSize = 0; 260 | 261 | return; 262 | } 263 | pimpl->writeBufferSize = pimpl->writeQueue.size(); 264 | pimpl->writeBuffer.reset(new char[pimpl->writeQueue.size()]); 265 | copy(pimpl->writeQueue.begin(), pimpl->writeQueue.end(), 266 | pimpl->writeBuffer.get()); 267 | pimpl->writeQueue.clear(); 268 | async_write(pimpl->port, asio::buffer(pimpl->writeBuffer.get(), pimpl->writeBufferSize), 269 | boost::bind(&AsyncSerial::writeEnd, this, asio::placeholders::error)); 270 | } 271 | else 272 | { 273 | setErrorStatus(true); 274 | doClose(); 275 | } 276 | } 277 | 278 | void AsyncSerial::doClose() 279 | { 280 | boost::system::error_code ec; 281 | pimpl->port.cancel(ec); 282 | if (ec) 283 | setErrorStatus(true); 284 | pimpl->port.close(ec); 285 | if (ec) 286 | setErrorStatus(true); 287 | } 288 | 289 | void AsyncSerial::setErrorStatus(bool e) 290 | { 291 | lock_guard l(pimpl->errorMutex); 292 | pimpl->error = e; 293 | } 294 | 295 | void AsyncSerial::setReadCallback(const boost::function &callback) 296 | { 297 | pimpl->callback = callback; 298 | } 299 | 300 | void AsyncSerial::clearReadCallback() 301 | { 302 | pimpl->callback.clear(); 303 | } 304 | 305 | #else //__APPLE__ 306 | 307 | #include 308 | #include 309 | #include 310 | #include 311 | #include 312 | 313 | class AsyncSerialImpl : private boost::noncopyable 314 | { 315 | public: 316 | AsyncSerialImpl() : backgroundThread(), open(false), error(false) {} 317 | 318 | boost::thread backgroundThread; ///< Thread that runs read operations 319 | bool open; ///< True if port open 320 | bool error; ///< Error flag 321 | mutable boost::mutex errorMutex; ///< Mutex for access to error 322 | 323 | int fd; ///< File descriptor for serial port 324 | 325 | char readBuffer[AsyncSerial::readBufferSize]; ///< data being read 326 | 327 | /// Read complete callback 328 | boost::function callback; 329 | }; 330 | 331 | AsyncSerial::AsyncSerial() : pimpl(new AsyncSerialImpl) 332 | { 333 | } 334 | 335 | AsyncSerial::AsyncSerial(const std::string &devname, unsigned int baud_rate, 336 | asio::serial_port_base::parity opt_parity, 337 | asio::serial_port_base::character_size opt_csize, 338 | asio::serial_port_base::flow_control opt_flow, 339 | asio::serial_port_base::stop_bits opt_stop) 340 | : pimpl(new AsyncSerialImpl) 341 | { 342 | open(devname, baud_rate, opt_parity, opt_csize, opt_flow, opt_stop); 343 | } 344 | 345 | void AsyncSerial::open(const std::string &devname, unsigned int baud_rate, 346 | asio::serial_port_base::parity opt_parity, 347 | asio::serial_port_base::character_size opt_csize, 348 | asio::serial_port_base::flow_control opt_flow, 349 | asio::serial_port_base::stop_bits opt_stop) 350 | { 351 | if (isOpen()) 352 | close(); 353 | 354 | setErrorStatus(true); //If an exception is thrown, error remains true 355 | 356 | struct termios new_attributes; 357 | speed_t speed; 358 | int status; 359 | 360 | // Open port 361 | pimpl->fd = ::open(devname.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); 362 | if (pimpl->fd < 0) 363 | throw(boost::system::system_error( 364 | boost::system::error_code(), "Failed to open port")); 365 | 366 | // Set Port parameters. 367 | status = tcgetattr(pimpl->fd, &new_attributes); 368 | if (status < 0 || !isatty(pimpl->fd)) 369 | { 370 | ::close(pimpl->fd); 371 | throw(boost::system::system_error( 372 | boost::system::error_code(), "Device is not a tty")); 373 | } 374 | new_attributes.c_iflag = IGNBRK; 375 | new_attributes.c_oflag = 0; 376 | new_attributes.c_lflag = 0; 377 | new_attributes.c_cflag = (CS8 | CREAD | CLOCAL); //8 data bit,Enable receiver,Ignore modem 378 | /* In non canonical mode (Ctrl-C and other disabled, no echo,...) VMIN and VTIME work this way: 379 | if the function read() has'nt read at least VMIN chars it waits until has read at least VMIN 380 | chars (even if VTIME timeout expires); once it has read at least vmin chars, if subsequent 381 | chars do not arrive before VTIME expires, it returns error; if a char arrives, it resets the 382 | timeout, so the internal timer will again start from zero (for the nex char,if any)*/ 383 | new_attributes.c_cc[VMIN] = 1; // Minimum number of characters to read before returning error 384 | new_attributes.c_cc[VTIME] = 1; // Set timeouts in tenths of second 385 | 386 | // Set baud rate 387 | switch (baud_rate) 388 | { 389 | case 50: 390 | speed = B50; 391 | break; 392 | case 75: 393 | speed = B75; 394 | break; 395 | case 110: 396 | speed = B110; 397 | break; 398 | case 134: 399 | speed = B134; 400 | break; 401 | case 150: 402 | speed = B150; 403 | break; 404 | case 200: 405 | speed = B200; 406 | break; 407 | case 300: 408 | speed = B300; 409 | break; 410 | case 600: 411 | speed = B600; 412 | break; 413 | case 1200: 414 | speed = B1200; 415 | break; 416 | case 1800: 417 | speed = B1800; 418 | break; 419 | case 2400: 420 | speed = B2400; 421 | break; 422 | case 4800: 423 | speed = B4800; 424 | break; 425 | case 9600: 426 | speed = B9600; 427 | break; 428 | case 19200: 429 | speed = B19200; 430 | break; 431 | case 38400: 432 | speed = B38400; 433 | break; 434 | case 57600: 435 | speed = B57600; 436 | break; 437 | case 115200: 438 | speed = B115200; 439 | break; 440 | case 230400: 441 | speed = B230400; 442 | break; 443 | default: 444 | { 445 | ::close(pimpl->fd); 446 | throw(boost::system::system_error( 447 | boost::system::error_code(), "Unsupported baud rate")); 448 | } 449 | } 450 | 451 | cfsetospeed(&new_attributes, speed); 452 | cfsetispeed(&new_attributes, speed); 453 | 454 | //Make changes effective 455 | status = tcsetattr(pimpl->fd, TCSANOW, &new_attributes); 456 | if (status < 0) 457 | { 458 | ::close(pimpl->fd); 459 | throw(boost::system::system_error( 460 | boost::system::error_code(), "Can't set port attributes")); 461 | } 462 | 463 | //These 3 lines clear the O_NONBLOCK flag 464 | status = fcntl(pimpl->fd, F_GETFL, 0); 465 | if (status != -1) 466 | fcntl(pimpl->fd, F_SETFL, status & ~O_NONBLOCK); 467 | 468 | setErrorStatus(false); //If we get here, no error 469 | pimpl->open = true; //Port is now open 470 | 471 | thread t(bind(&AsyncSerial::doRead, this)); 472 | pimpl->backgroundThread.swap(t); 473 | } 474 | 475 | bool AsyncSerial::isOpen() const 476 | { 477 | return pimpl->open; 478 | } 479 | 480 | bool AsyncSerial::errorStatus() const 481 | { 482 | lock_guard l(pimpl->errorMutex); 483 | return pimpl->error; 484 | } 485 | 486 | void AsyncSerial::close() 487 | { 488 | if (!isOpen()) 489 | return; 490 | 491 | pimpl->open = false; 492 | 493 | ::close(pimpl->fd); //The thread waiting on I/O should return 494 | 495 | pimpl->backgroundThread.join(); 496 | if (errorStatus()) 497 | { 498 | throw(boost::system::system_error(boost::system::error_code(), 499 | "Error while closing the device")); 500 | } 501 | } 502 | 503 | void AsyncSerial::write(const char *data, size_t size) 504 | { 505 | if (::write(pimpl->fd, data, size) != size) 506 | setErrorStatus(true); 507 | } 508 | 509 | void AsyncSerial::write(const std::vector &data) 510 | { 511 | if (::write(pimpl->fd, &data[0], data.size()) != data.size()) 512 | setErrorStatus(true); 513 | } 514 | 515 | void AsyncSerial::writeString(const std::string &s) 516 | { 517 | if (::write(pimpl->fd, &s[0], s.size()) != s.size()) 518 | setErrorStatus(true); 519 | } 520 | 521 | AsyncSerial::~AsyncSerial() 522 | { 523 | if (isOpen()) 524 | { 525 | try 526 | { 527 | close(); 528 | } 529 | catch (...) 530 | { 531 | //Don't throw from a destructor 532 | } 533 | } 534 | } 535 | 536 | void AsyncSerial::doRead() 537 | { 538 | //Read loop in spawned thread 539 | for (;;) 540 | { 541 | int received = ::read(pimpl->fd, pimpl->readBuffer, readBufferSize); 542 | if (received < 0) 543 | { 544 | if (isOpen() == false) 545 | return; //Thread interrupted because port closed 546 | else 547 | { 548 | setErrorStatus(true); 549 | continue; 550 | } 551 | } 552 | if (pimpl->callback) 553 | pimpl->callback(pimpl->readBuffer, received); 554 | } 555 | } 556 | 557 | void AsyncSerial::readEnd(const boost::system::error_code &error, 558 | size_t bytes_transferred) 559 | { 560 | //Not used 561 | } 562 | 563 | void AsyncSerial::doWrite() 564 | { 565 | //Not used 566 | } 567 | 568 | void AsyncSerial::writeEnd(const boost::system::error_code &error) 569 | { 570 | //Not used 571 | } 572 | 573 | void AsyncSerial::doClose() 574 | { 575 | //Not used 576 | } 577 | 578 | void AsyncSerial::setErrorStatus(bool e) 579 | { 580 | lock_guard l(pimpl->errorMutex); 581 | pimpl->error = e; 582 | } 583 | 584 | void AsyncSerial::setReadCallback(const function &callback) 585 | { 586 | pimpl->callback = callback; 587 | } 588 | 589 | void AsyncSerial::clearReadCallback() 590 | { 591 | pimpl->callback.clear(); 592 | } 593 | 594 | #endif //__APPLE__ 595 | 596 | // 597 | //Class CallbackAsyncSerial 598 | // 599 | 600 | CallbackAsyncSerial::CallbackAsyncSerial() : AsyncSerial() 601 | { 602 | } 603 | 604 | CallbackAsyncSerial::CallbackAsyncSerial(const std::string &devname, 605 | unsigned int baud_rate, 606 | asio::serial_port_base::parity opt_parity, 607 | asio::serial_port_base::character_size opt_csize, 608 | asio::serial_port_base::flow_control opt_flow, 609 | asio::serial_port_base::stop_bits opt_stop) 610 | : AsyncSerial(devname, baud_rate, opt_parity, opt_csize, opt_flow, opt_stop) 611 | { 612 | } 613 | 614 | void CallbackAsyncSerial::setCallback(const boost::function &callback) 615 | { 616 | setReadCallback(callback); 617 | } 618 | 619 | void CallbackAsyncSerial::clearCallback() 620 | { 621 | clearReadCallback(); 622 | } 623 | 624 | CallbackAsyncSerial::~CallbackAsyncSerial() 625 | { 626 | clearReadCallback(); 627 | } 628 | --------------------------------------------------------------------------------