测试gazebo中抓取cube程序正常运行

src/robot_ar_track/aruco_ros/aruco_ros/launch/single.launch

@@ -9,11 +9,11 @@
     <arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->
 
     <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_cam" 
-    args="0.1 0 0 0 0 0 base_footprint camera 10" />
+    args="0.16 0 0.05 0 0 0 base_footprint camera 10" />
 
     <node pkg="aruco_ros" type="single" name="aruco_single">
-        <remap from="/camera_info" to="/usb_cam/camera_info" />
-        <remap from="/image" to="/usb_cam/image_raw" />
+        <remap from="/camera_info" to="/robot/camera/camera_info" />
+        <remap from="/image" to="/robot/camera/image_raw" />
         <param name="image_is_rectified" value="True"/>
         <param name="marker_size"        value="$(arg markerSize)"/>
         <param name="marker_id"          value="$(arg markerId)"/>

src/robot_cube/cfg/param.yaml

@@ -1,15 +1,15 @@
 
-odom_frame: /odom
-base_frame: /base_link
+odom_frame: odom
+base_frame: base_footprint
 
 cmd_topic: /cmd_vel
 marker_topic: /aruco_single/marker
 yaw_service: /imu_node/GetYawData
 gripper_service: /mobilebase_arduino/gripper_angle
 
-angular_speed: 0.3
+angular_speed: 0.2
 linear_x_speed: 0.1
 linear_y_speed: 0.1
 gripper_srv_angle: 50
 tolerance_d: 0.005
-position_x_threshold: 0.1
+position_x_threshold: 0.0005

src/robot_cube/src/robot_cube_node.cpp

@@ -1,7 +1,7 @@
 /*
  * @Author: adam_zhuo
  * @Date: 2020-12-30 10:32:32
- * @LastEditTime: 2021-01-06 18:12:39
+ * @LastEditTime: 2021-01-07 18:06:03
  * @LastEditors: Please set LastEditors
  * @Description: grip cube
  * @FilePath: /blackTornadoRobot/src/robot_cube/src/robot_cube_node.cpp
@@ -131,12 +131,24 @@ void yaw_update(const float dynamic_turn_angle)
 void robot_turn(const float dynamic_turn_angle){
     tf::TransformListener tf_listener;
     tf::StampedTransform tf_transform;
+    
+    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 last_angle = fabs(tf::getYaw(tf_transform.getRotation()));
     float turn_angle = 0.0;
     while((fabs(turn_angle) < dynamic_turn_angle)&&(ros::ok()))
     {
-        pub_twist_cmd(0, 0, -angular_speed); //原地转弯,不需要linear_x,y的控制
+        pub_twist_cmd(0, 0, angular_speed); //原地转弯,不需要linear_x,y的控制
         ros::Duration(0.10).sleep(); //100 ms
 
         try
@@ -208,7 +220,7 @@ void robot_move(int move_flag,const float check_dist)
 }
 
 void find_cube(){
-    ros::Rate rate(10);
+    ros::Rate rate(100);
     bool status = ros::ok();
     while(status){
         pub_twist_cmd(0,0,angular_speed);
@@ -222,7 +234,7 @@ void find_cube(){
 
 void find_cube_2_control_angular(){
     //yaw_update(yaw);
-    robot_turn(fabs(yaw));
+    robot_turn(yaw);
 }
 
 void control_angular_2_control_distance(){
@@ -230,7 +242,7 @@ void control_angular_2_control_distance(){
     position_y = position_d * cos(yaw);
     ROS_INFO("position_x is %f",position_x);
     ROS_INFO("position_y is %f",position_y);
-    //robot_move(HORIZON_MOVE,position_x);
+    //robot_move(HORIZON_MOVE,-position_x);
     //robot_move(VERTICAL_MOVE,position_y);
 }
 
@@ -246,11 +258,13 @@ void rcv_marker_callback(const visualization_msgs::Marker & marker){
         return;
     }
     position_x = marker.pose.position.x;
-    if(abs(position_x) < position_x_threshold){
+    ROS_INFO("position_x is %f",position_x);
+    if(fabs(fabs(position_x)-0.0695) < position_x_threshold){
+        ROS_INFO("cha is %f",fabs(position_x)-0.0695);
         position_d = abs(marker.pose.position.y);
         tf::quaternionMsgToTF(marker.pose.orientation, quat);
         tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
-        yaw = abs(yaw) - 0.262;
+        yaw = abs(yaw);
         ROS_INFO("yaw is %f",yaw);
         find_cube_flag = true;
     }
@@ -286,15 +300,15 @@ int main(int argc, char** argv){
 
     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);
+    //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();
+    control_angular_2_control_distance();
     //grip_cube();
 
     return 0;

src/robot_description/urdf/robot.xacro

@@ -11,55 +11,8 @@
 
   <xacro:include filename="$(find robot_description)/urdf/robot_stack.xacro" />
   <!--<xacro include filename="$(find robot_description)/urdf/rplidar_a2.urdf" />-->
-  
-  <gazebo>
-    <plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
-      <commandTopic>cmd_vel</commandTopic>
-      <odometryTopic>odom</odometryTopic>
-      <odometryFrame>odom</odometryFrame>
-      <odometryRate>20.0</odometryRate>
-      <robotBaseFrame>base_footprint</robotBaseFrame>
-    </plugin>
-  </gazebo>
 
-  <!-- camera -->
-  <gazebo reference="camera_link">
-    <sensor type="camera" name="camera">
-      <update_rate>30.0</update_rate>
-      <camera name="head">
-        <horizontal_fov>1.3962634</horizontal_fov>
-        <image>
-          <width>320</width>
-          <height>240</height>
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.02</near>
-          <far>300</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <!-- Noise is sampled independently per pixel on each frame.
-               That pixel's noise value is added to each of its color
-               channels, which at that point lie in the range [0,1]. -->
-          <mean>0.0</mean>
-          <stddev>0.007</stddev>
-        </noise>
-      </camera>
-      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>0.0</updateRate>
-        <cameraName>robot/camera</cameraName>
-        <imageTopicName>image_raw</imageTopicName>
-        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-        <frameName>camera_link</frameName>
-        <hackBaseline>0.07</hackBaseline>
-        <distortionK1>0.0</distortionK1>
-        <distortionK2>0.0</distortionK2>
-        <distortionK3>0.0</distortionK3>
-        <distortionT1>0.0</distortionT1>
-        <distortionT2>0.0</distortionT2>
-      </plugin>
-    </sensor>
-  </gazebo>
+  <xacro:include filename="$(find robot_description)/urdf/robot_gazebo.xacro" />
+  
+  
 </robot>

src/robot_description/urdf/robot_gazebo.xacro

@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<robot name="cheng" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <gazebo>
+    <plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
+      <commandTopic>cmd_vel</commandTopic>
+      <odometryTopic>odom</odometryTopic>
+      <odometryFrame>odom</odometryFrame>
+      <odometryRate>20.0</odometryRate>
+      <robotBaseFrame>base_footprint</robotBaseFrame>
+    </plugin>
+  </gazebo>
+
+  <!-- camera -->
+  <gazebo reference="camera">
+    <sensor type="camera" name="camera">
+      <update_rate>30.0</update_rate>
+      <camera name="head">
+        <horizontal_fov>0.96</horizontal_fov>
+        <image>
+          <width>320</width>
+          <height>240</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <!-- Noise is sampled independently per pixel on each frame.
+               That pixel's noise value is added to each of its color
+               channels, which at that point lie in the range [0,1]. -->
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>0.0</updateRate>
+        <cameraName>robot/camera</cameraName>
+        <imageTopicName>image_raw</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <frameName>camera</frameName>
+        <hackBaseline>0.07</hackBaseline>
+        <distortionK1>0.172290</distortionK1>
+        <distortionK2>-0.414934</distortionK2>
+        <distortionK3>-0.000095</distortionK3>
+        <distortionT1>0.010100</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+  </gazebo>
+
+</robot>

src/robot_description/urdf/robot_stack.xacro

@@ -52,20 +52,20 @@
     <!-- camera joint -->
   <joint name="camera_joint" type="fixed">
     <origin xyz="0.16 0 0.05" rpy="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="camera_link"/>
+    <parent link="base_footprint"/>
+    <child link="camera"/>
   </joint>
 
   <!-- camera link -->
-  <link name="camera_link">
+  <link name="camera">
     <visual>
-     <origin xyz="0 0 0" rpy="1.57 0 1.57"/>
+     <origin xyz="0 0 0" rpy="0 1.57 0"/>
       <geometry>
        <box size="0.02 0.02 0.002"/>
       </geometry>
     </visual>
     <collision>
-      <origin xyz="0.003 0.065 0.007" rpy="0 0 0"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
         <box size="0.012 0.132 0.020"/>
       </geometry>