|
|
@@ -0,0 +1,268 @@
|
|
|
+/*
|
|
|
+ * @Author: adam_zhuo
|
|
|
+ * @Date: 2020-12-30 10:32:32
|
|
|
+ * @LastEditTime: 2020-12-31 19:00:46
|
|
|
+ * @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 "rasp_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;
|
|
|
+rasp_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<rasp_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;
|
|
|
+}
|
