删除无用的包;修改GetYawData

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/CMakeLists.txt

@@ -52,7 +52,7 @@ add_service_files(
   setYawZero.srv
   setBaudRate.srv
   setPinOutHL.srv
-  getYawData.srv
+  GetYawData.srv
 )
 
 ## Generate added messages and services with any dependencies listed here
@@ -91,7 +91,7 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-  INCLUDE_DIRS include
+#  INCLUDE_DIRS include
 #  LIBRARIES serial_imu_hat_6dof
 #  CATKIN_DEPENDS dynamic_reconfigure roscpp sensor_msgs
 #  DEPENDS system_lib

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/package.xml

@@ -51,15 +51,22 @@
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
 
   <build_export_depend>dynamic_reconfigure</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>message_runtime</build_export_depend>
 
   <exec_depend>dynamic_reconfigure</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_generation</exec_depend>
   <exec_depend>message_runtime</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -27,7 +27,7 @@
 #include "serial_imu_hat_6dof/setYawZero.h"
 #include "serial_imu_hat_6dof/setBaudRate.h"
 #include "serial_imu_hat_6dof/setPinOutHL.h"
-#include "serial_imu_hat_6dof/getYawData.h"
+#include "serial_imu_hat_6dof/GetYawData.h"
 
 
 static float g_yawData; //全局变量,存储当前yaw值,可以通过服务来得到该值
@@ -83,8 +83,8 @@ bool pinOutHLService(serial_imu_hat_6dof::setPinOutHL::Request &req,
     return true;
 }
 
-bool getYawDataService(serial_imu_hat_6dof::getYawData::Request &req,
-                       serial_imu_hat_6dof::getYawData::Response &res)
+bool GetYawDataService(serial_imu_hat_6dof::GetYawData::Request &req,
+                       serial_imu_hat_6dof::GetYawData::Response &res)
 {
     res.yaw = g_yawData;
     return true;
@@ -141,7 +141,7 @@ int main(int argc, char **argv)
     ros::ServiceServer setBaudSrv= handle.advertiseService(baud_update_service, setBaudService);
     ros::ServiceServer setyawSrv = handle.advertiseService(yaw_zero_service,  yawZeroService);
     ros::ServiceServer pinOutSrv = handle.advertiseService(pin_outHL_service, pinOutHLService);
-    ros::ServiceServer getYawSrv = handle.advertiseService(yaw_data_service,  getYawDataService);
+    ros::ServiceServer getYawSrv = handle.advertiseService(yaw_data_service,  GetYawDataService);
 
     ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
     ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 1);

src/robot_calibration/CMakeLists.txt

@@ -104,8 +104,8 @@ generate_dynamic_reconfigure_options(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-  LIBRARIES robot_calibration
-  CATKIN_DEPENDS roscpp rospy std_msgs dynamic_reconfigure
+#  LIBRARIES robot_calibration
+#  CATKIN_DEPENDS roscpp rospy std_msgs dynamic_reconfigure
 #  DEPENDS system_lib
 )
 
@@ -116,7 +116,7 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-# include
+  include
   ${catkin_INCLUDE_DIRS}
 )
 
@@ -143,7 +143,8 @@ add_executable(dynamic_tutorials src/pid_dynamic.cpp)
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-add_dependencies(dynamic_tutorials ${PROJECT_NAME}_gencfg)
+#add_dependencies(dynamic_tutorials ${PROJECT_NAME}_gencfg)
+add_dependencies(dynamic_tutorials ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 
 target_link_libraries(dynamic_tutorials ${catkin_LIBRARIES})

src/robot_cube/CMakeLists.txt

@@ -1,210 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(robot_cube)
-
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
-  nav_msgs
-  roscpp
-  visualization_msgs
-  tf
-  ros_arduino_msgs
-  rasp_imu_hat_6dof
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   geometry_msgs#   nav_msgs#   std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES robot_cube
-#  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/robot_cube.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-add_executable(${PROJECT_NAME}_node src/robot_cube_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}_node
-   ${catkin_LIBRARIES}
- )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_robot_cube.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)

src/robot_cube/cfg/param.yaml

@@ -1,15 +0,0 @@
-
-odom_frame: /odom_combined
-base_frame: /base_footprint
-
-cmd_topic: /cmd_vel
-marker_topic: /aruco_single/marker
-yaw_service: /imu_node/GetYawData
-gripper_service: /mobilebase_arduino/gripper_angle
-
-angular_speed: 0.3
-linear_x_speed: 0.1
-linear_y_speed: 0.08
-gripper_srv_angle: 50
-tolerance_d: 0.005
-position_x_threshold: 0.005

src/robot_cube/launch/robot_cube.launch

@@ -1,11 +0,0 @@
-<launch>
-
-  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_cam" 
-    args="0.1 0 0 0 0 0 base_footprint camera 10" />
-  <arg name="cfg_file" default="$(find robot_cube)/cfg/param.yaml" />
-
-  <node pkg="robot_cube" type="robot_cube_node" name="robot_cube_node" output="screen">
-    <rosparam file="$(arg cfg_file)" command="load" />
-  </node>
-  
-</launch>

src/robot_cube/package.xml

@@ -1,79 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>robot_cube</name>
-  <version>0.0.0</version>
-  <description>The robot_cube package</description>
-
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="tianbot@todo.todo">tianbot</maintainer>
-
-
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/robot_cube</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>visualization_msgs</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>ros_arduino_msgs</build_depend>
-  <build_depend>rasp_imu_hat_6dof</build_depend>
-  <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>nav_msgs</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>visualization_msgs</build_export_depend>
-  <build_export_depend>tf</build_export_depend>
-  <build_export_depend>ros_arduino_msgs</build_export_depend>
-  <build_export_depend>rasp_imu_hat_6dof</build_export_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>visualization_msgs</exec_depend>
-  <exec_depend>tf</exec_depend>
-  <exec_depend>ros_arduino_msgs</exec_depend>
-  <exec_depend>rasp_imu_hat_6dof</exec_depend>
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
-</package>

src/robot_cube/src/robot_cube_node.cpp

@@ -1,268 +0,0 @@
-/*
- * @Author: adam_zhuo
- * @Date: 2020-12-30 10:32:32
- * @LastEditTime: 2021-03-09 15:03:14
- * @LastEditors: Please set LastEditors
- * @Description: grip cube
- * @FilePath: /blackTornadoRobot/src/robot_cube/src/robot_cube_node.cpp
- */
-
-#include <cmath>
-#include <string>
-#include <thread>
-#include <ros/ros.h>
-#include <visualization_msgs/Marker.h>
-#include <geometry_msgs/Twist.h>
-#include <nav_msgs/Odometry.h>
-#include <tf/tf.h>
-#include <tf/transform_listener.h>
-#include "serial_imu_hat_6dof/GetYawData.h"
-#include "ros_arduino_msgs/GripperAngle.h"
-
-using namespace std;
-
-#define VERTICAL_MOVE  1
-#define HORIZON_MOVE  2
-
-double roll, pitch, yaw;
-float position_x, position_x_threshold, position_y, position_d, tolerance_d;
-float angular_speed, linear_x_speed, linear_y_speed;
-int gripper_srv_angle;
-
-bool find_cube_flag = false;
-
-string odom_frame = "/odom_combined";
-string base_frame = "/base_footprint";
-ros::Publisher cmd_pub;
-ros::Subscriber marker_sub;
-tf::Quaternion quat;
-ros::ServiceClient yaw_srv_client;
-ros::ServiceClient gripper_srv_client;
-serial_imu_hat_6dof::GetYawData yaw_srv;
-ros_arduino_msgs::GripperAngle gripper_srv;
-
-void pub_twist_cmd(float linear_x, float linear_y, float angular_z);
-void yaw_update(const float dynamic_turn_angle);
-void robot_move(int move_flag,const float check_dist);
-void find_cube();
-void find_cube_2_control_angular();
-void control_angular_2_control_distance();
-void grip_cube();
-void rcv_marker_callback(const visualization_msgs::Marker & marker);
-void run_spin_thread();
-
-static void toEulerAngle(const double x,const double y,const double z,const double w, double& roll, double& pitch, double& yaw)
-{
-// roll (x-axis rotation)
-    double sinr_cosp = -2.0 * (x * y - w * z);
-    double cosr_cosp = w * w + x * x -y * y - z * z;
-    yaw = atan2(sinr_cosp, cosr_cosp);
-
-// pitch (y-axis rotation)
-    double sinp = +2.0 * (x * z + w * y);
-    if (fabs(sinp) >= 1)
-        pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
-    else
-        pitch = asin(sinp);
-
-// yaw (z-axis rotation)
-    double siny_cosp = -2.0 * (y * z - w * x);
-    double cosy_cosp = w * w - x * x -y * y + z * z;
-    roll = atan2(siny_cosp, cosy_cosp);
-//    return yaw;
-}
-
-void pub_twist_cmd(float linear_x, float linear_y, float angular_z)
-{
-    geometry_msgs::Twist twist_msg;
-
-    twist_msg.linear.x = linear_x;
-    twist_msg.linear.y = linear_y;
-    twist_msg.linear.z = 0.0;
-
-    twist_msg.angular.x = 0.0;
-    twist_msg.angular.y = 0.0;
-    twist_msg.angular.z = angular_z;
-
-    cmd_pub.publish(twist_msg);
-}
-
-void yaw_update(const float dynamic_turn_angle)
-{
-    float start_yaw = 0.0;
-    float target = 0.0;
-    float diff = 10.0;
-    float now_yaw = 0.0;
-    float turn_yaw = 0.0;
-    
-    yaw_srv_client.call(yaw_srv);
-    start_yaw = yaw_srv.response.yaw;
-    turn_yaw = dynamic_turn_angle;
-    target = start_yaw + turn_yaw;
-    if(target > M_PI){
-        target = target - M_PI*2.0;
-    }else if(target < -M_PI){
-        target = target + M_PI*2.0;
-    }
-    
-    //开始不断的计算偏差,调整角度
-    while(fabs(diff) > 0.01){
-        yaw_srv_client.call(yaw_srv);
-        now_yaw = yaw_srv.response.yaw;
-        ROS_INFO("Now Yaw data:%f, Target Yaw:%f", now_yaw, target);
-
-        diff = target - now_yaw;
-        if(diff > 0){
-            pub_twist_cmd(0, 0, angular_speed);
-        }
-        else{
-            pub_twist_cmd(0, 0, -angular_speed);
-        }
-        ROS_WARN("Yaw diff:%f, angular_speed:%f", diff, angular_speed);
-        
-        ros::Duration(0.04).sleep();
-    }
-    pub_twist_cmd(0, 0, 0); //在修正角度结束后应该立刻停止小车转动
-    ROS_INFO("Turn Yaw diff over !");
-    ros::Duration(0.1).sleep(); //100 ms
-}
-
-void robot_move(int move_flag,const float check_dist)
-{
-    tf::TransformListener tf_listener;
-    tf::StampedTransform tf_transform;
-    float dist = 0.0; //每次前进移动的距离
-
-    try
-    {
-        tf_listener.waitForTransform(odom_frame, base_frame, ros::Time(0), ros::Duration(5.0));
-    }
-    catch(tf::TransformException &ex)
-    {
-        ROS_ERROR("tf_Listener.waitForTransform timeout error !");
-        ROS_ERROR("%s", ex.what());
-        ros::shutdown();
-    }
-
-    tf_listener.lookupTransform(odom_frame, base_frame, ros::Time(0), tf_transform);
-    float x_start = tf_transform.getOrigin().x();
-    float y_start = tf_transform.getOrigin().y();
-
-    bool dist_flag = true;
-    while(dist_flag&&(ros::ok()))
-    {
-        switch(move_flag){
-            case VERTICAL_MOVE:
-                pub_twist_cmd(copysign(linear_x_speed,check_dist), 0, 0);
-                break;
-            case HORIZON_MOVE:
-                pub_twist_cmd(0,copysign(linear_y_speed,check_dist), 0);
-                break;
-        }
-            
-        ros::Duration(0.10).sleep(); //100 ms
-
-        tf_listener.lookupTransform(odom_frame, base_frame, ros::Time(0),tf_transform);
-        float x = tf_transform.getOrigin().x();
-        float y = tf_transform.getOrigin().y();
-        dist = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));
-        ROS_INFO("Go forward, dist:%f", dist);
-        if(fabs(check_dist)-dist<tolerance_d){
-            dist_flag = false;
-        }
-    }
-    pub_twist_cmd(0, 0, 0); //stop move
-    ROS_INFO("Go forward finish !!!");
-}
-
-void find_cube(){
-    ros::Rate rate(10);
-    bool status = ros::ok();
-    while(status){
-        pub_twist_cmd(0,0,angular_speed);
-        if(find_cube_flag){
-            break;
-        }
-        rate.sleep();
-    }
-    pub_twist_cmd(0,0,0);
-}
-
-void find_cube_2_control_angular(){
-    yaw_update(yaw);
-}
-
-void control_angular_2_control_distance(){
-    position_x = (position_d + 0.16) * sin(yaw);
-    ROS_INFO("position_x is %f",position_x);
-    position_y = position_d * cos(yaw);
-    robot_move(HORIZON_MOVE,-position_x);
-    robot_move(VERTICAL_MOVE,position_y);
-}
-
-void grip_cube(){
-    gripper_srv.request.servoID = 1;
-    gripper_srv.request.angle = 50;
-    gripper_srv.request.delay = 0;
-    gripper_srv_client.call(gripper_srv);
-}
-
-void rcv_marker_callback(const visualization_msgs::Marker & marker){
-    if(find_cube_flag){
-        return;
-    }
-    position_x = marker.pose.position.x;
-    if(abs(position_x) < position_x_threshold){
-        position_d = abs(marker.pose.position.y);
-        tf::quaternionMsgToTF(marker.pose.orientation, quat);
-        tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
-        yaw = abs(yaw);
-        //yaw = abs(yaw) - 0.262;
-        ROS_INFO("yaw is %f",yaw);
-        find_cube_flag = true;
-    }
-}
-
-void run_spin_thread(){
-    ros::spin();
-}
-
-int main(int argc, char** argv){
-    string cmd_topic;
-    string marker_topic;
-    string yaw_service;
-    string gripper_service;
-
-    ros::init(argc, argv, "robot_cube_node");
-    ros::NodeHandle nh("~");
-
-    ros::param::get("~odom_frame", odom_frame);
-    ros::param::get("~base_frame", base_frame);
-
-    ros::param::get("~cmd_topic", cmd_topic);
-    ros::param::get("~marker_topic", marker_topic);
-    ros::param::get("~yaw_service", yaw_service);
-    ros::param::get("~gripper_service", gripper_service);
-
-    ros::param::get("~angular_speed", angular_speed);
-    ros::param::get("~linear_x_speed", linear_x_speed);
-    ros::param::get("~linear_y_speed", linear_y_speed);
-    ros::param::get("~gripper_srv_angle", gripper_srv_angle);
-    ros::param::get("~tolerance_d", tolerance_d);
-    ros::param::get("~position_x_threshold", position_x_threshold);
-
-    cmd_pub    = nh.advertise<geometry_msgs::Twist>(cmd_topic, 1);
-    marker_sub = nh.subscribe(marker_topic, 1, rcv_marker_callback);
-    yaw_srv_client = nh.serviceClient<serial_imu_hat_6dof::GetYawData>(yaw_service);
-    gripper_srv_client = nh.serviceClient<ros_arduino_msgs::GripperAngle>(gripper_service);
-
-    thread spin_thread(run_spin_thread);
-    spin_thread.detach();
-
-    find_cube();
-    find_cube_2_control_angular();
-    control_angular_2_control_distance();
-    //grip_cube();
-
-    return 0;
-}

src/robot_new_year/CMakeLists.txt

@@ -1,210 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(robot_new_year)
-
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  geometry_msgs
-  nav_msgs
-  visualization_msgs
-  tf
-  ros_arduino_msgs
-  rasp_imu_hat_6dof
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES robot_new_year
-#  CATKIN_DEPENDS roscpp
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/robot_new_year.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-add_executable(${PROJECT_NAME}_node src/robot_new_year_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}_node
-  ${catkin_LIBRARIES}
-)
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_robot_new_year.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)

src/robot_new_year/cfg/param.yaml

@@ -1,33 +0,0 @@
-robot_id: 1
-
-odom_frame: odom_combined
-base_frame: base_footprint
-
-cmd_topic: /cmd_vel
-voice_topic: /voice_system/iflytek_offline_tts_topic
-yaw_service: /imu_node/GetYawData
-gripper_service: /mobilebase_arduino/gripper_angle
-control_service: /mobilebase_arduino/gripper_control
-
-vertical_linear_x_speed: 0.1
-turn_angular_z_speed: -0.8
-curve_linear_x_speed: -0.06
-curve_angular_z_speed: 0.4
-diagonal_linear_x_speed: -0.1
-diagonal_linear_y_speed: -0.1
-
-first_vertical_distance: 0.1
-first_turn_yaw: 1.57
-second_turn_yaw: 0.35
-second_vertical_distance: 0.1
-diagonal_distance: 0.2
-gripper_open_angle: 65
-gripper_close_angle: 20
-
-tolerance_d: 0.005
-
-last_words: 我要甩掉二零二零年的霉气
-future_words: 祝全国人民新年快乐
-
-first_delay: 1
-second_delay: 3

src/robot_new_year/launch/robot_new_year.launch

@@ -1,9 +0,0 @@
-<launch>
-
-  <arg name="cfg_file" default="$(find robot_new_year)/cfg/param.yaml" />
-
-  <node pkg="robot_new_year" type="robot_new_year_node" name="robot_new_year_node" output="screen">
-    <rosparam file="$(arg cfg_file)" command="load" />
-  </node>
-  
-</launch>

src/robot_new_year/package.xml

@@ -1,77 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>robot_new_year</name>
-  <version>0.0.0</version>
-  <description>The robot_new_year package</description>
-
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="tianbot@todo.todo">tianbot</maintainer>
-
-
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/robot_new_year</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>ros_arduino_msgs</build_depend>
-  <build_depend>rasp_imu_hat_6dof</build_depend>
-  <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>nav_msgs</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>tf</build_export_depend>
-  <build_export_depend>ros_arduino_msgs</build_export_depend>
-  <build_export_depend>rasp_imu_hat_6dof</build_export_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>tf</exec_depend>
-  <exec_depend>ros_arduino_msgs</exec_depend>
-  <exec_depend>rasp_imu_hat_6dof</exec_depend>
-
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
-</package>

src/robot_new_year/src/robot_new_year_node.cpp

@@ -1,299 +0,0 @@
-/*
- * @Author: adam_zhuo
- * @Date: 2021-01-21 10:13:20
- * @LastEditTime: 2021-03-09 15:07:45
- * @LastEditors: Please set LastEditors
- * @Description: In User Settings Edit
- * @FilePath: /blackTornadoRobot/src/robot_new_year/src/robot_new_year_node.cpp
- */
-
-#include <string>
-#include <ros/ros.h>
-#include <std_msgs/String.h>
-#include <geometry_msgs/Twist.h>
-#include <nav_msgs/Odometry.h>
-#include <tf/tf.h>
-#include <tf/transform_listener.h>
-#include "serial_imu_hat_6dof/GetYawData.h"
-#include "ros_arduino_msgs/GripperAngle.h"
-#include "ros_arduino_msgs/GripperControl.h"
-
-using namespace std;
-
-#define VERTICAL_MOVE  1
-#define HORIZON_MOVE  2 
-#define DIAGONAL_MOVE 3
-#define ROBOT1 1
-#define ROBOT2 2
-
-ros::Publisher cmd_pub;
-ros::Publisher voice_pub;
-ros::ServiceClient yaw_srv_client;
-ros::ServiceClient gripper_srv_client;
-ros::ServiceClient control_srv_client;
-serial_imu_hat_6dof::GetYawData yaw_srv;
-ros_arduino_msgs::GripperAngle gripper_srv;
-ros_arduino_msgs::GripperControl control_srv;
-
-float vertical_linear_x_speed;
-float turn_angular_z_speed;
-float curve_linear_x_speed,curve_angular_z_speed;
-float diagonal_linear_x_speed,diagonal_linear_y_speed;
-
-float first_vertical_distance,second_vertical_distance;
-float diagonal_distance;
-int gripper_open_angle,gripper_close_angle;
-float initial_yaw,first_turn_yaw,second_turn_yaw;
-
-float tolerance_d;
-
-string odom_frame = "/odom_combined";
-string base_frame = "/base_footprint";
-
-void pub_twist_cmd(float linear_x, float linear_y, float angular_z);
-void yaw_update(const float dynamic_turn_angle);
-void robot_move(int move_flag,const float check_dist);
-void robot_curve();
-
-void pub_twist_cmd(float linear_x, float linear_y, float angular_z)
-{
-    geometry_msgs::Twist twist_msg;
-
-    twist_msg.linear.x = linear_x;
-    twist_msg.linear.y = linear_y;
-    twist_msg.linear.z = 0.0;
-
-    twist_msg.angular.x = 0.0;
-    twist_msg.angular.y = 0.0;
-    twist_msg.angular.z = angular_z;
-
-    cmd_pub.publish(twist_msg);
-}
-
-void yaw_update(const float dynamic_turn_angle)
-{
-    float start_yaw = 0.0;
-    float target = 0.0;
-    float diff = 10.0;
-    float now_yaw = 0.0;
-    float turn_yaw = 0.0;
-    
-    yaw_srv_client.call(yaw_srv);
-    start_yaw = yaw_srv.response.yaw;
-    turn_yaw = dynamic_turn_angle;
-    target = start_yaw + turn_yaw;
-    if(target > M_PI){
-        target = target - M_PI*2.0;
-    }else if(target < -M_PI){
-        target = target + M_PI*2.0;
-    }
-    
-    while(fabs(diff) > 0.01){
-        yaw_srv_client.call(yaw_srv);
-        now_yaw = yaw_srv.response.yaw;
-        diff = target - now_yaw;
-        if(diff > 0){
-            pub_twist_cmd(0, 0, turn_angular_z_speed);
-        }
-        else{
-            pub_twist_cmd(0, 0, -turn_angular_z_speed);
-        }
-        ros::Duration(0.04).sleep();
-    }
-    pub_twist_cmd(0, 0, 0);
-}
-
-void robot_move(int move_flag,const float check_dist)
-{
-    tf::TransformListener tf_listener;
-    tf::StampedTransform tf_transform;
-    float dist = 0.0; 
-
-    try
-    {
-        tf_listener.waitForTransform(odom_frame, base_frame, ros::Time(0), ros::Duration(5.0));
-    }
-    catch(tf::TransformException &ex)
-    {
-        ROS_ERROR("tf_Listener.waitForTransform timeout error !");
-        ROS_ERROR("%s", ex.what());
-        ros::shutdown();
-    }
-
-    tf_listener.lookupTransform(odom_frame, base_frame, ros::Time(0), tf_transform);
-    float x_start = tf_transform.getOrigin().x();
-    float y_start = tf_transform.getOrigin().y();
-
-    bool dist_flag = true;
-    while(dist_flag&&(ros::ok()))
-    {
-        switch(move_flag){
-            case VERTICAL_MOVE:
-                pub_twist_cmd(copysign(vertical_linear_x_speed,check_dist), 0, 0);
-                break;
-            case DIAGONAL_MOVE:
-                pub_twist_cmd(diagonal_linear_x_speed,diagonal_linear_y_speed,0);
-                break;
-        }
-            
-        ros::Duration(0.10).sleep(); //100 ms
-
-        tf_listener.lookupTransform(odom_frame, base_frame, ros::Time(0),tf_transform);
-        float x = tf_transform.getOrigin().x();
-        float y = tf_transform.getOrigin().y();
-        dist = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));
-        if(fabs(check_dist)-dist<tolerance_d){
-            dist_flag = false;
-        }
-    }
-    pub_twist_cmd(0, 0, 0); //stop move
-}
-
-void robot_turn(float target,float angular_z_speed){
-    float start_yaw;
-    float now_yaw;
-    bool flag = true;
-    yaw_srv_client.call(yaw_srv);
-    start_yaw = yaw_srv.response.yaw;
-    while(flag){
-        pub_twist_cmd(0, 0, angular_z_speed);
-        ros::Duration(0.005).sleep();
-        yaw_srv_client.call(yaw_srv);
-        now_yaw = yaw_srv.response.yaw;
-        if((target-fabs(now_yaw - start_yaw))<0.005){
-            flag = false;
-        }
-    }
-    pub_twist_cmd(0, 0, 0);
-}
-
-void robot_curve(){
-    float start_yaw;
-    float now_yaw;
-    bool flag = true;
-    yaw_srv_client.call(yaw_srv);
-    start_yaw = yaw_srv.response.yaw;
-    while(flag){
-        pub_twist_cmd(curve_linear_x_speed, 0, curve_angular_z_speed);
-        ros::Duration(0.1).sleep();
-        yaw_srv_client.call(yaw_srv);
-        now_yaw = yaw_srv.response.yaw;
-        if((1.57-fabs(now_yaw - start_yaw))<0.005){
-            flag = false;
-        }
-    }
-    pub_twist_cmd(0, 0, 0);
-}
-
-void gripper(int angle){
-    gripper_srv.request.servoID = 1;
-    gripper_srv.request.angle = angle;
-    gripper_srv.request.delay = 0;
-    gripper_srv_client.call(gripper_srv);
-}
-
-void control(){
-    control_srv.request.servoID = 0;
-    control_srv.request.value = 1;
-    control_srv_client.call(control_srv);
-    ros::Duration(2).sleep();
-    control_srv.request.value = 3;
-    control_srv_client.call(control_srv);
-    ros::Duration(2).sleep();
-    control_srv.request.value = 2;
-    control_srv_client.call(control_srv);
-}
-
-void voice(string words){
-    std_msgs::String lucky;
-    lucky.data = words;
-    voice_pub.publish(lucky);
-}
-
-int main(int argc, char** argv){
-    string cmd_topic;
-    string voice_topic;
-    string yaw_service;
-    string gripper_service;
-    string control_service;
-    string last_words;
-    string future_words;
-    float first_delay;
-    float second_delay;
-    int robot_id;
-
-    ros::init(argc, argv, "robot_new_year_node");
-    ros::NodeHandle nh("~");
-
-    ros::param::get("~robot_id",robot_id);
-    ros::param::get("~odom_frame", odom_frame);
-    ros::param::get("~base_frame", base_frame);
-
-    ros::param::get("~cmd_topic", cmd_topic);
-    ros::param::get("~voice_topic", voice_topic);
-    ros::param::get("~yaw_service", yaw_service);
-    ros::param::get("~gripper_service", gripper_service);
-    ros::param::get("~control_service", control_service);
-
-    ros::param::get("~vertical_linear_x_speed", vertical_linear_x_speed);
-    ros::param::get("~turn_angular_z_speed", turn_angular_z_speed);
-    ros::param::get("~curve_linear_x_speed", curve_linear_x_speed);
-    ros::param::get("~curve_angular_z_speed", curve_angular_z_speed);
-    ros::param::get("~diagonal_linear_x_speed", diagonal_linear_x_speed);
-    ros::param::get("~diagonal_linear_y_speed", diagonal_linear_y_speed);
-
-    ros::param::get("~first_vertical_distance", first_vertical_distance);
-    ros::param::get("~second_vertical_distance", second_vertical_distance);
-    ros::param::get("~first_turn_yaw", first_turn_yaw);
-    ros::param::get("~second_turn_yaw", second_turn_yaw);
-    ros::param::get("~diagonal_distance", diagonal_distance);
-    ros::param::get("~gripper_open_angle", gripper_open_angle);
-    ros::param::get("~gripper_close_angle", gripper_close_angle);
-    
-    ros::param::get("~tolerance_d", tolerance_d);
-
-    ros::param::get("~last_words", last_words);
-    ros::param::get("~future_words", future_words);
-
-    ros::param::get("~first_delay", first_delay);
-    ros::param::get("~second_delay", second_delay);
-
-    
-    cmd_pub    = nh.advertise<geometry_msgs::Twist>(cmd_topic, 1);
-    voice_pub = nh.advertise<std_msgs::String>(voice_topic, 1);
-    yaw_srv_client = nh.serviceClient<serial_imu_hat_6dof::GetYawData>(yaw_service);
-    gripper_srv_client = nh.serviceClient<ros_arduino_msgs::GripperAngle>(gripper_service);
-    control_srv_client = nh.serviceClient<ros_arduino_msgs::GripperControl>(control_service);
-
-    yaw_srv_client.call(yaw_srv);
-    initial_yaw = yaw_srv.response.yaw;
-
-    if(robot_id == ROBOT2){
-        turn_angular_z_speed = -turn_angular_z_speed;
-        curve_angular_z_speed = -curve_angular_z_speed;
-        diagonal_linear_y_speed = -diagonal_linear_y_speed;
-    }
-    ros::Duration(first_delay).sleep();
-    robot_move(VERTICAL_MOVE,first_vertical_distance);
-    robot_turn(first_turn_yaw,turn_angular_z_speed);
-    if(robot_id == ROBOT1){
-        voice(last_words);
-    }
-    ros::Duration(second_delay).sleep();
-    robot_turn(second_turn_yaw,turn_angular_z_speed);
-    robot_turn(second_turn_yaw,-turn_angular_z_speed);
-    robot_turn(second_turn_yaw,turn_angular_z_speed);
-    robot_turn(second_turn_yaw,-turn_angular_z_speed);
-    gripper(gripper_open_angle);
-    robot_curve();
-    robot_move(VERTICAL_MOVE,second_vertical_distance);
-    gripper(gripper_close_angle);
-    control();
-    robot_move(DIAGONAL_MOVE,diagonal_distance);
-    if(robot_id == ROBOT1){
-        voice(future_words);
-    }
-
-    ros::spin();
-    return 0;
-}