拍摄视频的程序

src/robot_new_year/cfg/param_client.yaml

@@ -1,5 +1,3 @@
-robot_id: 1
-
 odom_frame: /robot1/odom_combined
 base_frame: /robot1/base_footprint
 
@@ -9,25 +7,32 @@ 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
+vertical_linear_x_speed: 0.08
+horizon_linear_y_speed: 0.1
+turn_angular_z_speed: -0.4
 curve_linear_x_speed: -0.06
-curve_angular_z_speed: 0.4
+curve_angular_z_speed: 0.3
 diagonal_linear_x_speed: -0.1
-diagonal_linear_y_speed: -0.1
+diagonal_linear_y_speed: -0.05
 
-first_vertical_distance: 0.1
+first_vertical_distance: 0.3
+first_horizon_distance: -0.05
 first_turn_yaw: 1.57
 second_turn_yaw: 0.35
-second_vertical_distance: 0.1
-diagonal_distance: 0.2
+second_vertical_distance: 0.255
+diagonal_distance: 0.35
 gripper_open_angle: 65
-gripper_close_angle: 20
+gripper_close_angle: 10
+third_vertical_distance: -0.03
+forth_vertical_distance: 0.05
+fifth_vertical_distance: -0.03
 
 tolerance_d: 0.005
 
 last_words: 我要甩掉二零二零年的霉气
 future_words: 祝全国人民新年快乐
 
-first_delay: 1
-second_delay: 3
+first_delay: 2
+second_delay: 1
+third_delay: 8
+forth_delay: 7

src/robot_new_year/cfg/param_server.yaml

@@ -1,5 +1,3 @@
-robot_id: 1
-
 odom_frame: /robot2/odom_combined
 base_frame: /robot2/base_footprint
 
@@ -9,25 +7,32 @@ 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
+vertical_linear_x_speed: 0.08
+horizon_linear_y_speed: 0.1
+turn_angular_z_speed: 0.4
 curve_linear_x_speed: -0.06
-curve_angular_z_speed: 0.4
+curve_angular_z_speed: -0.3
 diagonal_linear_x_speed: -0.1
-diagonal_linear_y_speed: -0.1
+diagonal_linear_y_speed: 0.05
 
-first_vertical_distance: 0.1
+first_vertical_distance: 0.3
+first_horizon_distance: 0.05
 first_turn_yaw: 1.57
 second_turn_yaw: 0.35
-second_vertical_distance: 0.1
-diagonal_distance: 0.2
+second_vertical_distance: 0.255
+diagonal_distance: 0.35
 gripper_open_angle: 65
-gripper_close_angle: 20
+gripper_close_angle: 0
+third_vertical_distance: 0.03
+forth_vertical_distance: -0.06
+fifth_vertical_distance: 0.02
 
 tolerance_d: 0.005
 
 last_words: 我要甩掉二零二零年的霉气
 future_words: 祝全国人民新年快乐
 
-first_delay: 1
-second_delay: 3
+first_delay: 2
+second_delay: 1
+third_delay: 8
+forth_delay: 7

src/robot_new_year/src/robot_new_year_client_node.cpp

@@ -1,7 +1,7 @@
 /*
  * @Author: adam_zhuo
  * @Date: 2021-01-23 11:08:51
- * @LastEditTime: 2021-01-23 15:04:00
+ * @LastEditTime: 2021-01-26 09:47:04
  * @LastEditors: Please set LastEditors
  * @Description: In User Settings Edit
  * @FilePath: /blackTornadoRobot/src/robot_new_year/src/robot_new_year_client_node.cpp
@@ -48,20 +48,28 @@ ros_arduino_msgs::GripperAngle gripper_srv;
 ros_arduino_msgs::GripperControl control_srv;
 
 int task;
-int task_state = 0;
+int client_task_state = 0;
+int server_task_state = 0;
 
 float vertical_linear_x_speed;
+float horizon_linear_y_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 first_vertical_distance,second_vertical_distance,first_horizon_distance;
+float third_vertical_distance,forth_vertical_distance,fifth_vertical_distance;
 float diagonal_distance;
 int gripper_open_angle,gripper_close_angle;
-float first_turn_yaw,second_turn_yaw;
+float first_turn_yaw,second_turn_yaw,initial_yaw;
 
 float tolerance_d;
 
+float first_delay;
+float second_delay;
+float third_delay;
+float forth_delay;
+
 string odom_frame = "/odom_combined";
 string base_frame = "/base_footprint";
 
@@ -111,6 +119,9 @@ void robot_move(int move_flag,const float check_dist)
             case VERTICAL_MOVE:
                 pub_twist_cmd(copysign(vertical_linear_x_speed,check_dist), 0, 0);
                 break;
+            case HORIZON_MOVE:
+                pub_twist_cmd(0,copysign(horizon_linear_y_speed,check_dist),0);
+                break;
             case DIAGONAL_MOVE:
                 pub_twist_cmd(diagonal_linear_x_speed,diagonal_linear_y_speed,0);
                 break;
@@ -145,20 +156,52 @@ void robot_turn(float target,float angular_z_speed){
         }
     }
     pub_twist_cmd(0, 0, 0);
+    // bool flag = true;
+    // float now_yaw,start_yaw,target_yaw;
+    // yaw_srv_client.call(yaw_srv);
+    // now_yaw = yaw_srv.response.yaw;
+    // start_yaw = now_yaw;
+    // target_yaw = fabs(start_yaw - initial_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_yaw-fabs(now_yaw - start_yaw))<0.005){
+    //         flag = false;
+    //     }
+    // }
+    // pub_twist_cmd(0, 0, 0);
 }
 
 void robot_curve(){
-    float start_yaw;
-    float now_yaw;
+    // 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);
     bool flag = true;
+    float now_yaw,start_yaw,target_yaw;
     yaw_srv_client.call(yaw_srv);
-    start_yaw = yaw_srv.response.yaw;
+    now_yaw = yaw_srv.response.yaw;
+    start_yaw = now_yaw;
+    target_yaw = fabs(start_yaw - initial_yaw);
     while(flag){
         pub_twist_cmd(curve_linear_x_speed, 0, curve_angular_z_speed);
-        ros::Duration(0.1).sleep();
+        ros::Duration(0.005).sleep();
         yaw_srv_client.call(yaw_srv);
         now_yaw = yaw_srv.response.yaw;
-        if((1.57-fabs(now_yaw - start_yaw))<0.005){
+        if((target_yaw-fabs(now_yaw - start_yaw))<0.005){
             flag = false;
         }
     }
@@ -178,22 +221,22 @@ void voice(string words){
     voice_pub.publish(lucky);
 }
 
-void control_up(){
+void control_up(int time){
     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;
+    ros::Duration(time).sleep();
+    control_srv.request.value = 2;
     control_srv_client.call(control_srv);
 }
 
-void control_down(){
+void control_down(int time){
     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);
+    ros::Duration(time).sleep();
+    control_srv.request.value = 2;
+    control_srv_client.call(control_srv);
 }
 
 /*
@@ -202,7 +245,7 @@ void control_down(){
 void doneCb(const actionlib::SimpleClientGoalState& state, const robot_new_year::new_yearResultConstPtr& result)
 {
     ROS_INFO("DONE");
-    task_state = 1;
+    server_task_state = 1;
     // ros::shutdown();
 }
 
@@ -219,31 +262,56 @@ void activeCb()
         break;
     case 2:
         robot_turn(first_turn_yaw,turn_angular_z_speed);
-        voice(last_words);
         break;
     case 3:
+        ros::Duration(first_delay).sleep();
+        break;
+    case 4:
         robot_turn(second_turn_yaw,turn_angular_z_speed);
+        break;
+    case 5:
         robot_turn(second_turn_yaw,-turn_angular_z_speed);
+        break;
+    case 6:
         robot_turn(second_turn_yaw,turn_angular_z_speed);
+        break;
+    case 7:
         robot_turn(second_turn_yaw,-turn_angular_z_speed);
         gripper(gripper_open_angle);
         break;
-    case 4:
+    case 8:
+        control_down(second_delay);
+        break;
+    case 9:
         robot_curve();
         break;
-    case 5:
+    case 10:
         robot_move(VERTICAL_MOVE,second_vertical_distance);
         break;
-    case 6:
+    case 11:
         gripper(gripper_close_angle);
-        control_up();
+        control_up(third_delay);
         break;
-    case 7:
+    case 12:
+        robot_move(HORIZON_MOVE,first_horizon_distance);
+        break;
+    case 13:
         robot_move(DIAGONAL_MOVE,diagonal_distance);
-        control_down();
-        voice(future_words);
+        break;
+    case 14:
+        control_down(forth_delay);
+        break;
+    case 15:
+        robot_move(VERTICAL_MOVE,third_vertical_distance);
+        break;
+    case 16:
+        robot_move(VERTICAL_MOVE,forth_vertical_distance);
+        break;
+    case 17:
+        robot_move(VERTICAL_MOVE,fifth_vertical_distance);
         break;
     }
+    client_task_state = 1;
 }
 
 /*
@@ -262,9 +330,6 @@ int main(int argc, char **argv)
     string gripper_service;
     string control_service;
     
-    float first_delay;
-    float second_delay;
-    
     ros::init(argc, argv, "robot_new_year_client_node");
     ros::NodeHandle nh("~");
 
@@ -276,13 +341,18 @@ int main(int argc, char **argv)
     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("~horizon_linear_y_speed", horizon_linear_y_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("~first_horizon_distance", first_horizon_distance);
     ros::param::get("~second_vertical_distance", second_vertical_distance);
+    ros::param::get("~third_vertical_distance", third_vertical_distance);
+    ros::param::get("~forth_vertical_distance", forth_vertical_distance);
+    ros::param::get("~fifth_vertical_distance", fifth_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);
@@ -293,6 +363,8 @@ int main(int argc, char **argv)
     ros::param::get("~future_words", future_words);
     ros::param::get("~first_delay", first_delay);
     ros::param::get("~second_delay", second_delay);
+    ros::param::get("~third_delay", third_delay);
+    ros::param::get("~forth_delay", forth_delay);
 
     cmd_pub    = nh.advertise<geometry_msgs::Twist>(cmd_topic, 1);
     voice_pub = nh.advertise<std_msgs::String>(voice_topic, 1);
@@ -300,22 +372,25 @@ int main(int argc, char **argv)
     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;
+
     Client client("action_new_year", true);
     client.waitForServer();
     robot_new_year::new_yearGoal new_year_goal;
-    int i;
-    for(i = 1;i < 8;i++)
+    for(int i = 1;i < 18;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)){
+            if(client_task_state&&server_task_state){
                 break;
             }
         }
-        task_state = 0;
+        client_task_state = 0;
+        server_task_state = 0;
     }
     
     ros::spin();

src/robot_new_year/src/robot_new_year_server_node.cpp

@@ -1,7 +1,7 @@
 /*
  * @Author: adam_zhuo
  * @Date: 2021-01-23 11:08:19
- * @LastEditTime: 2021-01-23 14:59:50
+ * @LastEditTime: 2021-01-25 19:06:54
  * @LastEditors: Please set LastEditors
  * @Description: In User Settings Edit
  * @FilePath: /blackTornadoRobot/src/robot_new_year/src/robot_new_year_server_node.cpp
@@ -45,17 +45,24 @@ ros_arduino_msgs::GripperAngle gripper_srv;
 ros_arduino_msgs::GripperControl control_srv;
 
 float vertical_linear_x_speed;
+float horizon_linear_y_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 first_vertical_distance,second_vertical_distance,first_horizon_distance;
+float third_vertical_distance,forth_vertical_distance,fifth_vertical_distance;
 float diagonal_distance;
 int gripper_open_angle,gripper_close_angle;
-float first_turn_yaw,second_turn_yaw;
+float first_turn_yaw,second_turn_yaw,initial_yaw;
 
 float tolerance_d;
 
+float first_delay;
+float second_delay;
+float third_delay;
+float forth_delay;
+
 string odom_frame = "/odom_combined";
 string base_frame = "/base_footprint";
 
@@ -102,6 +109,9 @@ void robot_move(int move_flag,const float check_dist)
             case VERTICAL_MOVE:
                 pub_twist_cmd(copysign(vertical_linear_x_speed,check_dist), 0, 0);
                 break;
+            case HORIZON_MOVE:
+                pub_twist_cmd(0,copysign(horizon_linear_y_speed,check_dist),0);
+                break;
             case DIAGONAL_MOVE:
                 pub_twist_cmd(diagonal_linear_x_speed,diagonal_linear_y_speed,0);
                 break;
@@ -136,20 +146,52 @@ void robot_turn(float target,float angular_z_speed){
         }
     }
     pub_twist_cmd(0, 0, 0);
+    // bool flag = true;
+    // float now_yaw,start_yaw,target_yaw;
+    // yaw_srv_client.call(yaw_srv);
+    // now_yaw = yaw_srv.response.yaw;
+    // start_yaw = now_yaw;
+    // target_yaw = fabs(start_yaw - initial_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_yaw-fabs(now_yaw - start_yaw))<0.005){
+    //         flag = false;
+    //     }
+    // }
+    // pub_twist_cmd(0, 0, 0);
 }
 
 void robot_curve(){
-    float start_yaw;
-    float now_yaw;
+    // 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);
     bool flag = true;
+    float now_yaw,start_yaw,target_yaw;
     yaw_srv_client.call(yaw_srv);
-    start_yaw = yaw_srv.response.yaw;
+    now_yaw = yaw_srv.response.yaw;
+    start_yaw = now_yaw;
+    target_yaw = fabs(start_yaw - initial_yaw);
     while(flag){
         pub_twist_cmd(curve_linear_x_speed, 0, curve_angular_z_speed);
-        ros::Duration(0.1).sleep();
+        ros::Duration(0.005).sleep();
         yaw_srv_client.call(yaw_srv);
         now_yaw = yaw_srv.response.yaw;
-        if((1.57-fabs(now_yaw - start_yaw))<0.005){
+        if((target_yaw-fabs(now_yaw - start_yaw))<0.005){
             flag = false;
         }
     }
@@ -169,31 +211,28 @@ void voice(string words){
     voice_pub.publish(lucky);
 }
 
-void control_up(){
+void control_up(int time){
     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;
+    ros::Duration(time).sleep();
+    control_srv.request.value = 2;
     control_srv_client.call(control_srv);
 }
 
-void control_down(){
+void control_down(int time){
     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);
+    ros::Duration(time).sleep();
+    control_srv.request.value = 2;
+    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);
@@ -202,25 +241,51 @@ void execute(const robot_new_year::new_yearGoalConstPtr& new_year_goal, Server*
         robot_turn(first_turn_yaw,turn_angular_z_speed);
         break;
     case 3:
+        ros::Duration(first_delay).sleep();
+        break;
+    case 4:
         robot_turn(second_turn_yaw,turn_angular_z_speed);
+        break;
+    case 5:
         robot_turn(second_turn_yaw,-turn_angular_z_speed);
+        break;
+    case 6:
         robot_turn(second_turn_yaw,turn_angular_z_speed);
+        break;
+    case 7:
         robot_turn(second_turn_yaw,-turn_angular_z_speed);
         gripper(gripper_open_angle);
         break;
-    case 4:
+    case 8:
+        control_down(second_delay);
+        break;
+    case 9:
         robot_curve();
         break;
-    case 5:
+    case 10:
         robot_move(VERTICAL_MOVE,second_vertical_distance);
         break;
-    case 6:
+    case 11:
         gripper(gripper_close_angle);
-        control_up();
+        control_up(third_delay);
         break;
-    case 7:
+    case 12:
+        robot_move(HORIZON_MOVE,first_horizon_distance);
+        break;
+    case 13:
         robot_move(DIAGONAL_MOVE,diagonal_distance);
-        control_down();
+        break;
+    case 14:
+        control_down(forth_delay);
+        break;
+    case 15:
+        robot_move(VERTICAL_MOVE,third_vertical_distance);
+        break;
+    case 16:
+        robot_move(VERTICAL_MOVE,forth_vertical_distance);
+        break;
+    case 17:
+        robot_move(VERTICAL_MOVE,fifth_vertical_distance);
         break;
     }
     as -> setSucceeded();  
@@ -235,11 +300,9 @@ int main(int argc, char **argv)
     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::NodeHandle nh;
 
     ros::param::get("~odom_frame", odom_frame);
     ros::param::get("~base_frame", base_frame);
@@ -249,13 +312,18 @@ int main(int argc, char **argv)
     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("~horizon_linear_y_speed", horizon_linear_y_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("~first_horizon_distance", first_horizon_distance);
     ros::param::get("~second_vertical_distance", second_vertical_distance);
+    ros::param::get("~third_vertical_distance", third_vertical_distance);
+    ros::param::get("~forth_vertical_distance", forth_vertical_distance);
+    ros::param::get("~fifth_vertical_distance", fifth_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);
@@ -266,6 +334,8 @@ int main(int argc, char **argv)
     ros::param::get("~future_words", future_words);
     ros::param::get("~first_delay", first_delay);
     ros::param::get("~second_delay", second_delay);
+    ros::param::get("~third_delay", third_delay);
+    ros::param::get("~forth_delay", forth_delay);
 
     cmd_pub    = nh.advertise<geometry_msgs::Twist>(cmd_topic, 1);
     voice_pub = nh.advertise<std_msgs::String>(voice_topic, 1);
@@ -273,6 +343,9 @@ int main(int argc, char **argv)
     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;
+
     Server server(nh, "action_new_year", boost::bind(&execute, _1, &server), false);
     server.start();