新增action

src/robot_new_year/CMakeLists.txt

@@ -15,6 +15,8 @@ find_package(catkin REQUIRED COMPONENTS
   tf
   ros_arduino_msgs
   rasp_imu_hat_6dof
+  actionlib_msgs
+  actionlib
 )
 
 ## System dependencies are found with CMake's conventions
@@ -65,17 +67,16 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
+add_action_files(
+  FILES
+  new_year.action
+)
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs  # Or other packages containing msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -139,21 +140,28 @@ include_directories(
 ## 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}
 )
 
+add_executable(${PROJECT_NAME}_server_node src/robot_new_year_server_node.cpp)
+
+add_dependencies(${PROJECT_NAME}_server_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(${PROJECT_NAME}_server_node
+  ${catkin_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_client_node src/robot_new_year_client_node.cpp)
+
+add_dependencies(${PROJECT_NAME}_client_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(${PROJECT_NAME}_client_node
+  ${catkin_LIBRARIES}
+)
+
 #############
 ## Install ##
 #############

src/robot_new_year/action/new_year.action

@@ -0,0 +1,5 @@
+int32 goal
+---
+bool finish
+---
+float32 complete_percent

src/robot_new_year/cfg/param_client.yaml

@@ -0,0 +1,33 @@
+robot_id: 1
+
+odom_frame: /robot1/odom_combined
+base_frame: /robot1/base_footprint
+
+cmd_topic: /robot1/cmd_vel
+voice_topic: /robot1/voice_system/iflytek_offline_tts_topic
+yaw_service: /robot1/imu_node/GetYawData
+gripper_service: /robot1/mobilebase_arduino/gripper_angle
+control_service: /robot1/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/cfg/param_server.yaml

@@ -0,0 +1,33 @@
+robot_id: 1
+
+odom_frame: /robot2/odom_combined
+base_frame: /robot2/base_footprint
+
+cmd_topic: /robot2/cmd_vel
+voice_topic: /robot2/voice_system/iflytek_offline_tts_topic
+yaw_service: /robot2/imu_node/GetYawData
+gripper_service: /robot2/mobilebase_arduino/gripper_angle
+control_service: /robot2/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_client.launch

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

src/robot_new_year/launch/robot_new_year_server.launch

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

src/robot_new_year/package.xml

@@ -55,18 +55,24 @@
   <build_depend>tf</build_depend>
   <build_depend>ros_arduino_msgs</build_depend>
   <build_depend>rasp_imu_hat_6dof</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</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>
+  <build_export_depend>actionlib</build_export_depend>
+  <build_export_depend>actionlib_msgs</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>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/robot_new_year/src/robot_new_year_client_node.cpp

@@ -0,0 +1,323 @@
+/*
+ * @Author: adam_zhuo
+ * @Date: 2021-01-23 11:08:51
+ * @LastEditTime: 2021-01-23 15:04:00
+ * @LastEditors: Please set LastEditors
+ * @Description: In User Settings Edit
+ * @FilePath: /blackTornadoRobot/src/robot_new_year/src/robot_new_year_client_node.cpp
+ */
+/*
+ *action客户端
+ */
+#include <ros/ros.h>
+#include <string>
+#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 <actionlib/client/simple_action_client.h>
+#include "robot_new_year/new_yearAction.h"  
+#include "rasp_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 FIRST_HORIZON_MOVE 1
+#define ROBOT_TURN 2
+#define ROBOT_THROW 3
+#define ROBOT_CURVE 4
+#define SECOND_HORIZON_MOVE 5
+#define ROBOT_GRIP 6
+#define ROBOT_DIAGONAL 7
+
+typedef actionlib::SimpleActionClient<robot_new_year::new_yearAction> Client;
+
+ros::Publisher cmd_pub;
+ros::Publisher voice_pub;
+ros::ServiceClient yaw_srv_client;
+ros::ServiceClient gripper_srv_client;
+ros::ServiceClient control_srv_client;
+rasp_imu_hat_6dof::GetYawData yaw_srv;
+ros_arduino_msgs::GripperAngle gripper_srv;
+ros_arduino_msgs::GripperControl control_srv;
+
+int task;
+int task_state = 0;
+
+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 first_turn_yaw,second_turn_yaw;
+
+float tolerance_d;
+
+string odom_frame = "/odom_combined";
+string base_frame = "/base_footprint";
+
+string last_words;
+string future_words;
+
+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 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 voice(string words){
+    std_msgs::String lucky;
+    lucky.data = words;
+    voice_pub.publish(lucky);
+}
+
+void control_up(){
+    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);
+}
+
+void control_down(){
+    control_srv.request.servoID = 0;
+    control_srv.request.value = 2;
+    control_srv_client.call(control_srv);
+    ros::Duration(2).sleep();
+    control_srv.request.value = 3;
+    control_srv_client.call(control_srv);
+}
+
+/*
+ *action完成时的回调函数，一次性
+ */
+void doneCb(const actionlib::SimpleClientGoalState& state, const robot_new_year::new_yearResultConstPtr& result)
+{
+    ROS_INFO("DONE");
+    task_state = 1;
+    // ros::shutdown();
+}
+
+/*
+ *action启动时的回调函数，一次性
+ */
+void activeCb()
+{
+    ROS_INFO("ACTIVE");
+    switch (task)
+    {
+    case 1:
+        robot_move(VERTICAL_MOVE,first_vertical_distance);
+        break;
+    case 2:
+        robot_turn(first_turn_yaw,turn_angular_z_speed);
+        voice(last_words);
+        break;
+    case 3:
+        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);
+        break;
+    case 4:
+        robot_curve();
+        break;
+    case 5:
+        robot_move(VERTICAL_MOVE,second_vertical_distance);
+        break;
+    case 6:
+        gripper(gripper_close_angle);
+        control_up();
+        break;
+    case 7:
+        robot_move(DIAGONAL_MOVE,diagonal_distance);
+        control_down();
+        voice(future_words);
+        break;
+    }
+}
+
+/*
+ *action收到反馈时的回调函数
+ */
+void feedbackCb(const robot_new_year::new_yearFeedbackConstPtr& feedback)
+{
+    ROS_INFO("THE NUMBER RIGHT NOM IS: %f", feedback -> complete_percent);
+}
+
+int main(int argc, char **argv)
+{
+    string cmd_topic;
+    string voice_topic;
+    string yaw_service;
+    string gripper_service;
+    string control_service;
+    
+    float first_delay;
+    float second_delay;
+    
+    ros::init(argc, argv, "robot_new_year_client_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("~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<rasp_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);
+
+    Client client("action_new_year", true);
+    client.waitForServer();
+    robot_new_year::new_yearGoal new_year_goal;
+    int i;
+    for(i = 1;i < 8;i++)
+    {
+        task = i;
+        new_year_goal.goal = task;  
+        client.sendGoal(new_year_goal, &doneCb, &activeCb, &feedbackCb);
+        while(true){
+            ros::Duration(0.01).sleep();
+            if(task_state&&(task<7)){
+                break;
+            }
+        }
+        task_state = 0;
+    }
+    
+    ros::spin();
+    return 0;
+}

src/robot_new_year/src/robot_new_year_node.cpp

@@ -1,7 +1,7 @@
 /*
  * @Author: adam_zhuo
  * @Date: 2021-01-21 10:13:20
- * @LastEditTime: 2021-01-22 15:23:19
+ * @LastEditTime: 2021-01-23 13:52:32
  * @LastEditors: Please set LastEditors
  * @Description: In User Settings Edit
  * @FilePath: /blackTornadoRobot/src/robot_new_year/src/robot_new_year_node.cpp
@@ -51,7 +51,6 @@ 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();
 
@@ -70,39 +69,6 @@ void pub_twist_cmd(float linear_x, float linear_y, float 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;
@@ -192,11 +158,17 @@ void gripper(int angle){
     gripper_srv_client.call(gripper_srv);
 }
 
-void control(){
+void control_up(){
     control_srv.request.servoID = 0;
     control_srv.request.value = 1;
     control_srv_client.call(control_srv);
     ros::Duration(2).sleep();
+    control_srv.request.value = 2;
+    control_srv_client.call(control_srv);
+}
+
+void control_down(){
+    control_srv.request.servoID = 0;
     control_srv.request.value = 3;
     control_srv_client.call(control_srv);
     ros::Duration(2).sleep();
@@ -288,8 +260,9 @@ int main(int argc, char** argv){
     robot_curve();
     robot_move(VERTICAL_MOVE,second_vertical_distance);
     gripper(gripper_close_angle);
-    control();
+    control_up();
     robot_move(DIAGONAL_MOVE,diagonal_distance);
+    control_down();
     if(robot_id == ROBOT1){
         voice(future_words);
     }

src/robot_new_year/src/robot_new_year_server_node.cpp

@@ -0,0 +1,281 @@
+/*
+ * @Author: adam_zhuo
+ * @Date: 2021-01-23 11:08:19
+ * @LastEditTime: 2021-01-23 14:59:50
+ * @LastEditors: Please set LastEditors
+ * @Description: In User Settings Edit
+ * @FilePath: /blackTornadoRobot/src/robot_new_year/src/robot_new_year_server_node.cpp
+ */
+#include <ros/ros.h>
+#include <string>
+#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 <actionlib/server/simple_action_server.h>
+#include "robot_new_year/new_yearAction.h"  
+#include "rasp_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 FIRST_HORIZON_MOVE 1
+#define ROBOT_TURN 2
+#define ROBOT_THROW 3
+#define ROBOT_CURVE 4
+#define SECOND_HORIZON_MOVE 5
+#define ROBOT_GRIP 6
+#define ROBOT_DIAGONAL 7
+
+typedef actionlib::SimpleActionServer<robot_new_year::new_yearAction> Server;
+
+ros::Publisher cmd_pub;
+ros::Publisher voice_pub;
+ros::ServiceClient yaw_srv_client;
+ros::ServiceClient gripper_srv_client;
+ros::ServiceClient control_srv_client;
+rasp_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 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)
+{
+    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 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 voice(string words){
+    std_msgs::String lucky;
+    lucky.data = words;
+    voice_pub.publish(lucky);
+}
+
+void control_up(){
+    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);
+}
+
+void control_down(){
+    control_srv.request.servoID = 0;
+    control_srv.request.value = 2;
+    control_srv_client.call(control_srv);
+    ros::Duration(2).sleep();
+    control_srv.request.value = 3;
+    control_srv_client.call(control_srv);
+}
+
+void execute(const robot_new_year::new_yearGoalConstPtr& new_year_goal, Server* as)
+{
+    robot_new_year::new_yearFeedback feedback;  
+    ROS_INFO("THE GOAL IS: %d", new_year_goal -> goal);
+    // feedback.complete_percent = 2;
+    // as -> publishFeedback(feedback);
+    // r.sleep();
+    switch(new_year_goal -> goal){
+    case 1:
+        robot_move(VERTICAL_MOVE,first_vertical_distance);
+        break;
+    case 2:
+        robot_turn(first_turn_yaw,turn_angular_z_speed);
+        break;
+    case 3:
+        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);
+        break;
+    case 4:
+        robot_curve();
+        break;
+    case 5:
+        robot_move(VERTICAL_MOVE,second_vertical_distance);
+        break;
+    case 6:
+        gripper(gripper_close_angle);
+        control_up();
+        break;
+    case 7:
+        robot_move(DIAGONAL_MOVE,diagonal_distance);
+        control_down();
+        break;
+    }
+    as -> setSucceeded();  
+}
+
+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;
+
+    ros::init(argc, argv, "robot_new_year_server_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("~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<rasp_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);
+
+    Server server(nh, "action_new_year", boost::bind(&execute, _1, &server), false);
+    server.start();
+
+    ros::spin();
+    return 0;
+}