From 1c4870007c0fc15ddf4e818309677bfa86caf967 Mon Sep 17 00:00:00 2001 From: "litian.zhuang" Date: Sat, 27 Mar 2021 13:48:27 +0800 Subject: [PATCH] add the spark_test --- onekey.sh | 29 +++-- src/spark/spark_test/CMakeLists.txt | 25 ++-- .../spark_test/launch/all_run_test.launch | 28 +++++ .../spark_test/launch/all_run_test_st.launch | 28 +++++ src/spark/spark_test/package.xml | 4 +- .../spark_test/src/spark_test_motion.cpp | 15 +-- .../spark_test/src/spark_turn_left_right.cpp | 106 ++++++++++++++++ .../src/spark_turn_left_right_st.cpp | 116 ++++++++++++++++++ .../spark_voice/scripts/baidu_asr.py | 6 +- 9 files changed, 325 insertions(+), 32 deletions(-) create mode 100755 src/spark/spark_test/launch/all_run_test.launch create mode 100755 src/spark/spark_test/launch/all_run_test_st.launch create mode 100644 src/spark/spark_test/src/spark_turn_left_right.cpp create mode 100644 src/spark/spark_test/src/spark_turn_left_right_st.cpp diff --git a/onekey.sh b/onekey.sh index d5f4e1e..0b859d2 100755 --- a/onekey.sh +++ b/onekey.sh @@ -695,16 +695,25 @@ spark_dock(){ } -spark_test(){ - - echo -e "${Info}" - echo -e "${Info} SPARK test" +spark_test_mode(){ PROJECTPATH=$(cd `dirname $0`; pwd) - source ${PROJECTPATH}/devel/setup.bash - - echo && stty erase ^? && read -p "按回车键（Enter）开始：" - print_command "roslaunch spark_test turn_test.launch" - roslaunch spark_test turn_test.launch + source ${PROJECTPATH}/devel/setup.bash + echo -e "${Info}老化测试程序，请将机器人放在一个50X50CM的方格中间进行测试，请选择： + 1.执行5分钟的检测功能； + 2.执行老化检测功能，直至断电。" + echo -e "${Info}退出请输入：Ctrl + c " + echo && stty erase ^? && read -p "请选择 1 或 2 ：" chnum + case "$chnum" in + 1) + roslaunch spark_test all_run_test_st.launch camera_type_tel:=${CAMERATYPE} + ;; + 2) + roslaunch spark_test all_run_test.launch camera_type_tel:=${CAMERATYPE} + ;; + *) + echo -e "${Error} 退出!" + ;; + esac } #让SPARK使用深度摄像头绘制地图 @@ -1003,7 +1012,7 @@ case "$num" in spark_dock ;; 98) - spark_test + spark_test_mode ;; *) echo -e "${Error} 请输入正确的数字 " diff --git a/src/spark/spark_test/CMakeLists.txt b/src/spark/spark_test/CMakeLists.txt index 2c23d67..86578eb 100644 --- a/src/spark/spark_test/CMakeLists.txt +++ b/src/spark/spark_test/CMakeLists.txt @@ -9,8 +9,8 @@ find_package(catkin REQUIRED COMPONENTS rospy std_msgs pcl_ros - tf spark_base + spark_carry_object ) @@ -18,12 +18,10 @@ find_package(catkin REQUIRED COMPONENTS ## catkin specific configuration ## ###################################= catkin_package( - LIBRARIES - CATKIN_DEPENDS roscpp rospy std_msgs tf - DEPENDS system_lib -) -include_directories(include - ${catkin_INCLUDE_DIRS} +# INCLUDE_DIRS include +# LIBRARIES spark_test +# CATKIN_DEPENDS roscpp rospy std_msgs spark_base +# DEPENDS system_lib ) ########### @@ -52,6 +50,13 @@ add_dependencies(spark_test_topic_node target_link_libraries(spark_test_topic_node ${catkin_LIBRARIES}) +add_executable(spark_turn_left_right_node src/spark_turn_left_right.cpp) +add_dependencies(spark_turn_left_right_node + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ${PROJECT_NAME}_generate_messages_cpp) +target_link_libraries(spark_turn_left_right_node ${catkin_LIBRARIES}) + add_executable(spark_test_motion_node src/spark_test_motion.cpp) add_dependencies(spark_test_motion_node ${${PROJECT_NAME}_EXPORTED_TARGETS} @@ -60,6 +65,12 @@ add_dependencies(spark_test_motion_node target_link_libraries(spark_test_motion_node ${catkin_LIBRARIES}) +add_executable(spark_turn_left_right_st_node src/spark_turn_left_right_st.cpp) +add_dependencies(spark_turn_left_right_st_node + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ${PROJECT_NAME}_generate_messages_cpp) +target_link_libraries(spark_turn_left_right_st_node ${catkin_LIBRARIES}) ############# ## Install ## diff --git a/src/spark/spark_test/launch/all_run_test.launch b/src/spark/spark_test/launch/all_run_test.launch new file mode 100755 index 0000000..ce35425 --- /dev/null +++ b/src/spark/spark_test/launch/all_run_test.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark/spark_test/launch/all_run_test_st.launch b/src/spark/spark_test/launch/all_run_test_st.launch new file mode 100755 index 0000000..a7dd56e --- /dev/null +++ b/src/spark/spark_test/launch/all_run_test_st.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark/spark_test/package.xml b/src/spark/spark_test/package.xml index 073cee0..24ff5d4 100644 --- a/src/spark/spark_test/package.xml +++ b/src/spark/spark_test/package.xml @@ -48,9 +48,7 @@ rospy std_msgs spark_base - tf - tf - + diff --git a/src/spark/spark_test/src/spark_test_motion.cpp b/src/spark/spark_test/src/spark_test_motion.cpp index 7e20018..6c080a1 100644 --- a/src/spark/spark_test/src/spark_test_motion.cpp +++ b/src/spark/spark_test/src/spark_test_motion.cpp @@ -1,11 +1,11 @@ #include "ros/ros.h" - +#include +#include #include #include #include #include -#include -#include + typedef pcl::PointCloud PointCloud; class MotionTest @@ -33,11 +33,9 @@ public: bool testMotion() { ros::Rate rate(10); - while(ros::ok) - { double prev_sec = ros::Time().now().toSec(); int sec = 10; - while (ros::ok) + while (1) { //逆时针旋转 if (ros::Time().now().toSec() - prev_sec > sec) @@ -47,7 +45,7 @@ public: rate.sleep(); } prev_sec = ros::Time().now().toSec(); - while (ros::ok) + while (1) { //顺时针旋转 if (ros::Time().now().toSec() - prev_sec > sec) @@ -57,7 +55,7 @@ public: rate.sleep(); } prev_sec = ros::Time().now().toSec(); -/* while (1) + while (1) { //逆时针同心圆旋转 if (ros::Time().now().toSec() - prev_sec > sec) @@ -76,7 +74,6 @@ public: ROS_INFO("shunshizhen tongxinyuan"); testTurnBody(0.2, -1, 10); rate.sleep(); - }*/ } } diff --git a/src/spark/spark_test/src/spark_turn_left_right.cpp b/src/spark/spark_test/src/spark_turn_left_right.cpp new file mode 100644 index 0000000..1d5b914 --- /dev/null +++ b/src/spark/spark_test/src/spark_turn_left_right.cpp @@ -0,0 +1,106 @@ +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include "spark_carry_object/status.h" +#include "spark_carry_object/position.h" +typedef pcl::PointCloud PointCloud; + +class MotionTest +{ +public: + ros::NodeHandle nhandle; + boost::thread* test_motion_thread_; + ros::Publisher cmd_pub; + ros::Publisher pump_pub; + ros::Publisher pos_pub; + MotionTest(ros::NodeHandle in_nh) + { + nhandle = in_nh; + + cmd_pub = nhandle.advertise("/cmd_vel", 1); + pump_pub = nhandle.advertise("pump_topic", 1); + pos_pub = nhandle.advertise("position_write_topic", 1); + test_motion_thread_ = new boost::thread(boost::bind(&MotionTest::testMotion, this)); + } + + ~MotionTest() + { + spark_carry_object::status onoff; + onoff.status = 0; + pump_pub.publish(onoff); + if (test_motion_thread_ != NULL) + { + delete test_motion_thread_; + } + } + + bool testMotion() + { + spark_carry_object::status onoff; + spark_carry_object::position pos; + ros::Rate rate(10); + onoff.status = 1; + pump_pub.publish(onoff); + pos.x = 120; + pos.y = 0; + pos.z = 35; + sleep(5); + pos_pub.publish(pos); + while(ros::ok) + { + pump_pub.publish(onoff); + double prev_sec = ros::Time().now().toSec(); + int sec = 10; + ROS_INFO("ni shi zhen"); + while (ros::ok) + { + //逆时针旋转 + if (ros::Time().now().toSec() - prev_sec > sec) + break; + + testTurnBody(0, 2, 10); + rate.sleep(); + } + ROS_INFO("shen shi zhen"); + prev_sec = ros::Time().now().toSec(); + while (ros::ok) + { + //顺时针旋转 + if (ros::Time().now().toSec() - prev_sec > sec) + break; + + testTurnBody(0, -2, 10); + rate.sleep(); + } + } + onoff.status = 0; + pump_pub.publish(onoff); + } + + void testTurnBody(float linearx, float angularz, float sec) + { + geometry_msgs::Twist vel; + vel.linear.x = linearx; + vel.angular.z = angularz; + cmd_pub.publish(vel); + } +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "spark_test_node"); + ROS_INFO("Spark test bring up"); + + ros::NodeHandle n; + MotionTest test(n); + + // start to test the spark + // test.testTopicStatus(); + ROS_INFO("Spark test stop"); + ros::spin(); + return 0; +} diff --git a/src/spark/spark_test/src/spark_turn_left_right_st.cpp b/src/spark/spark_test/src/spark_turn_left_right_st.cpp new file mode 100644 index 0000000..1cdb059 --- /dev/null +++ b/src/spark/spark_test/src/spark_turn_left_right_st.cpp @@ -0,0 +1,116 @@ +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include "spark_carry_object/status.h" +#include "spark_carry_object/position.h" +typedef pcl::PointCloud PointCloud; + +class MotionTest +{ +public: + ros::NodeHandle nhandle; + boost::thread* test_motion_thread_; + ros::Publisher cmd_pub; + ros::Publisher pump_pub; + ros::Publisher pos_pub; + MotionTest(ros::NodeHandle in_nh) + { + nhandle = in_nh; + + cmd_pub = nhandle.advertise("/cmd_vel", 1); + pump_pub = nhandle.advertise("pump_topic", 1); + pos_pub = nhandle.advertise("position_write_topic", 1); + test_motion_thread_ = new boost::thread(boost::bind(&MotionTest::testMotion, this)); + } + + ~MotionTest() + { + spark_carry_object::status onoff; + onoff.status = 0; + pump_pub.publish(onoff); + if (test_motion_thread_ != NULL) + { + delete test_motion_thread_; + } + } + + bool testMotion() + { + spark_carry_object::status onoff; + spark_carry_object::position pos; + ros::Rate rate(10); + onoff.status = 1; + pump_pub.publish(onoff); + pos.x = 120; + pos.y = 0; + pos.z = 35; + sleep(5); + pos_pub.publish(pos); + double start_sec = ros::Time().now().toSec(); + int keep_sec = 300; + ROS_INFO("time is start! start turning."); + while(ros::ok) + { + pump_pub.publish(onoff); + double prev_sec = ros::Time().now().toSec(); + int sec = 10; + ROS_INFO("ni shi zhen"); + while (ros::ok) + { + //逆时针旋转 + if (ros::Time().now().toSec() - prev_sec > sec) + break; + + testTurnBody(0, 2, 10); + rate.sleep(); + } + ROS_INFO("shen shi zhen"); + prev_sec = ros::Time().now().toSec(); + while (ros::ok) + { + + //顺时针旋转 + if (ros::Time().now().toSec() - prev_sec > sec) + break; + + testTurnBody(0, -2, 10); + rate.sleep(); + } + if (ros::Time().now().toSec() - start_sec > keep_sec) + { + onoff.status = 0; + pump_pub.publish(onoff); + ROS_WARN("time is over! stop turning."); + break; + } + } + + } + + void testTurnBody(float linearx, float angularz, float sec) + { + geometry_msgs::Twist vel; + vel.linear.x = linearx; + vel.angular.z = angularz; + cmd_pub.publish(vel); + } +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "spark_test_node"); + ROS_INFO("Spark test bring up"); + + ros::NodeHandle n; + MotionTest test(n); + + // start to test the spark + // test.testTopicStatus(); + //ROS_INFO("Spark test stop"); + ros::spin(); + return 0; +} diff --git a/src/spark_app/spark_voice/scripts/baidu_asr.py b/src/spark_app/spark_voice/scripts/baidu_asr.py index 39ca131..b777189 100755 --- a/src/spark_app/spark_voice/scripts/baidu_asr.py +++ b/src/spark_app/spark_voice/scripts/baidu_asr.py @@ -8,9 +8,9 @@ from threading import Thread, Event from lib import Microphone,AipSpeech """ 你的 APPID AK SK """ -APP_ID = '10475584' -API_KEY = 'm2zeD2S8ft8z6Y0oUblsIZhy' -SECRET_KEY = 'Y226Y4YCRzpYXy3E3MTG0xxyqRGZYG50' +APP_ID = '23797700' +API_KEY = 'dK8rz5DG9pPi90cgQVtx1ddn' +SECRET_KEY = 'X1lzayj5RQwfkoLHXHd5DALRvFZpjhBP' def main(): #logging.basicConfig(level=logging.DEBUG)