|
|
@@ -0,0 +1,299 @@
|
|
|
+/*
|
|
|
+ * @Author: adam_zhuo
|
|
|
+ * @Date: 2021-01-21 10:13:20
|
|
|
+ * @LastEditTime: 2021-01-22 15:23:19
|
|
|
+ * @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 "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 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;
|
|
|
+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 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<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);
|
|
|
+
|
|
|
+ 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;
|
|
|
+}
|
