|
|
@@ -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);
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
}
|
|
|
|
|
|
void robot_curve(){
|
|
|
- float start_yaw;
|
|
|
- float now_yaw;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
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;
|
|
|
|
|
|
}
|
|
|
|
|
|
@@ -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();
|
