Thiết kế website giá rẻ

Question

Currently, I am working on implementing ROS(ros-noetic) control for a four-wheel mecanum car. I initially attempted to move the mecanum car with the vehicle as the center of coordinates, but this approach resulted in it appearing to drift, which is impractical for real-world applications of mecanum cars. Consequently, I’ve started simulating the actual movement behavior of the mecanum car. However, I’m unsure if the issue lies with the plugin or the code itself, as the simulated car in Gazebo isn’t moving at all. How can I resolve this issue? Are there any other reference materials you could recommend?

Below are the Python file for controlling the vehicle’s movement, the xacro file for the vehicle body, and the Gazebo file.
mecanum_point.py

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist, Point
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from std_srvs.srv import SetBool, SetBoolResponse
import math
import numpy as np

# Robot state variables
position_ = Point()
yaw_ = 0.0

# Mission state variables
state_ = 0
active_ = False

# Waypoints for navigation
waypoints = [
    Point(5.0, 5.0, 1.5708),    
    Point(-5.0, 5.0, 3.1416),   
    Point(5.0, -5.0, -1.5708),  
    Point(-5.0, -5.0, 0.0),     
    Point(-5.0, 0.0, 1.5708),   
    Point(0.0, 0.0, 0.0)        
]

# Parameters for control
yaw_precision_ = math.pi / 90.0  # +/- 2 degrees allowed
dist_precision_ = 0.3  # Distance precision in meters

# ROS publishers and subscribers
pub_left_front = None
pub_right_front = None
pub_left_rear = None
pub_right_rear = None

def go_to_point_switch(req):
    global active_
    active_ = req.data
    res = SetBoolResponse()
    res.success = True
    res.message = 'Done'
    return res

def normalize_angle(angle):
    while angle > math.pi:
        angle -= 2.0 * math.pi
    while angle < -math.pi:
        angle += 2.0 * math.pi
    return angle

def change_state(state):
    global state_
    state_ = state
    rospy.loginfo('State changed to [%s]' % state_)

def calculate_wheel_velocities(des_pos):
    global yaw_, yaw_precision_, position_

    err_yaw = normalize_angle(des_pos.z - yaw_)
    err_pos_x = des_pos.x - position_.x
    err_pos_y = des_pos.y - position_.y

    # Robot frame error calculation
    robot_err_x = np.cos(yaw_) * err_pos_x + np.sin(yaw_) * err_pos_y
    robot_err_y = -np.sin(yaw_) * err_pos_x + np.cos(yaw_) * err_pos_y

    # Control coefficients
    k_rho = 0.5
    k_alpha = 1.0

    # Calculate desired velocities for each wheel
    vel_left_front = k_rho * (robot_err_x - robot_err_y) + k_alpha * err_yaw
    vel_right_front = k_rho * (robot_err_x + robot_err_y) - k_alpha * err_yaw
    vel_left_rear = k_rho * (robot_err_x + robot_err_y) + k_alpha * err_yaw
    vel_right_rear = k_rho * (robot_err_x - robot_err_y) - k_alpha * err_yaw

    return vel_left_front, vel_right_front, vel_left_rear, vel_right_rear

def publish_wheel_velocities(vel_left_front, vel_right_front, vel_left_rear, vel_right_rear):
    twist_left_front = Twist()
    twist_left_front.linear.x = vel_left_front

    twist_right_front = Twist()
    twist_right_front.linear.x = vel_right_front

    twist_left_rear = Twist()
    twist_left_rear.linear.x = vel_left_rear

    twist_right_rear = Twist()
    twist_right_rear.linear.x = vel_right_rear

    pub_left_front.publish(twist_left_front)
    pub_right_front.publish(twist_right_front)
    pub_left_rear.publish(twist_left_rear)
    pub_right_rear.publish(twist_right_rear)

def done():
    publish_wheel_velocities(0.0, 0.0, 0.0, 0.0)

def clbk_odom(msg):
    global position_, yaw_

    position_ = msg.pose.pose.position

    quaternion = (
        msg.pose.pose.orientation.x,
        msg.pose.pose.orientation.y,
        msg.pose.pose.orientation.z,
        msg.pose.pose.orientation.w)
    _, _, yaw_ = euler_from_quaternion(quaternion)

def main():
    global pub_left_front, pub_right_front, pub_left_rear, pub_right_rear, active_

    rospy.init_node('go_to_point_omni')
    pub_left_front = rospy.Publisher('/left_front_wheel/cmd_vel', Twist, queue_size=1)
    pub_right_front = rospy.Publisher('/right_front_wheel/cmd_vel', Twist, queue_size=1)
    pub_left_rear = rospy.Publisher('/left_rear_wheel/cmd_vel', Twist, queue_size=1)
    pub_right_rear = rospy.Publisher('/right_rear_wheel/cmd_vel', Twist, queue_size=1)

    sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom)
    srv = rospy.Service('/go_to_point_switch', SetBool, go_to_point_switch)
    rate = rospy.Rate(20)

    current_waypoint_index = 0

    while not rospy.is_shutdown():
        if not active_:
            rate.sleep()
            continue

        if state_ == 0:  # Moving to waypoint
            current_waypoint = waypoints[current_waypoint_index]
            vel_left_front, vel_right_front, vel_left_rear, vel_right_rear = calculate_wheel_velocities(current_waypoint)
            publish_wheel_velocities(vel_left_front, vel_right_front, vel_left_rear, vel_right_rear)
            # Check if the robot reached the current waypoint
            if math.sqrt((current_waypoint.x - position_.x)**2 + (current_waypoint.y - position_.y)**2) < dist_precision_:
                if abs(normalize_angle(current_waypoint.z - yaw_)) < yaw_precision_:
                    change_state(1)

        elif state_ == 1:  # Arrived at waypoint
            done()
            current_waypoint_index += 1
            if current_waypoint_index >= len(waypoints):
                rospy.loginfo('All waypoints visited')
                active_ = False
            else:
                change_state(0)

        else:
            rospy.logerr('Unknown state!')

        rate.sleep()

if __name__ == '__main__':
    main()

robot.xacro

<?xml version="1.0"?>
<!-- ###################################### -->
<!-- DESCRIPTION OF THE 4-WHEELED ROBOT     -->
<!-- ###################################### -->

<robot name="mecanum_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Body dimensions -->
  <xacro:property name="body_link_x_dim" value="1"/>
  <xacro:property name="body_link_y_dim" value="0.6"/>
  <xacro:property name="body_link_z_dim" value="0.3"/>

  <!-- Wheel dimensions -->
  <xacro:property name="wheel_link_radius" value="0.15"/>
  <xacro:property name="wheel_link_length" value="0.1"/>
  <xacro:property name="wheel_link_z_location" value="-0.1"/>

  <!-- Material density -->
  <xacro:property name="body_density" value="2710.0"/>
  <xacro:property name="wheel_density" value="2710.0"/>

  <!-- Pi constant -->
  <xacro:property name="pi_constant" value="3.14159265"/>

  <!-- Robot body and wheel mass -->
  <xacro:property name="body_mass" value="${body_density*body_link_x_dim*body_link_y_dim*body_link_z_dim}"/>
  <xacro:property name="wheel_mass" value="${wheel_density*pi_constant*wheel_link_radius*wheel_link_radius*wheel_link_length}"/>

  <!-- Moments of inertia of the wheel -->
  <xacro:property name="Iz_wheel" value="${0.5*wheel_mass*wheel_link_radius*wheel_link_radius}"/>
  <xacro:property name="I_wheel" value="${(1.0/12.0)*wheel_mass*(3.0*wheel_link_radius*wheel_link_radius+wheel_link_length*wheel_link_length)}"/>

  <!-- This macro defines the complete inertial section of the wheel -->
  <!-- It is used later in the code                                   -->
  <xacro:macro name="inertia_wheel">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="${wheel_mass}"/>
      <inertia ixx="${I_wheel}" ixy="0" ixz="0" iyy="${I_wheel}" iyz="0" izz="${Iz_wheel}" />
    </inertial>
  </xacro:macro>

  <!-- Include the file that defines extra Gazebo options and motion control driver -->
  <xacro:include filename="$(find ros_car)/urdf/robot.gazebo" />

  <!-- ###################################### -->
  <!-- DEFINE LINKS, JOINTS    -->
  <!-- ###################################### -->

  <!-- Dummy link otherwise Gazebo will complain -->
  <link name="dummy"/>
  <joint name="dummy_joint" type="fixed">
    <parent link="dummy"/>
    <child link="body_link"/>
  </joint>

  <!-- ###################################### -->
  <!-- START: BODY link of the robot    -->
  <!-- ###################################### -->
  <link name="body_link">
    <visual>
      <geometry>
        <box size="${body_link_x_dim} ${body_link_y_dim} ${body_link_z_dim}" />
      </geometry>
      <material name="red">
            <color rgba="1 0 0 1"/>
      </material>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <box size="${body_link_x_dim} ${body_link_y_dim} ${body_link_z_dim}" />
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="${body_mass}"/>
      <inertia ixx="${(1/12)*body_mass*(body_link_y_dim*body_link_y_dim+body_link_z_dim*body_link_z_dim)}"
               ixy="0" ixz="0"
               iyy="${(1/12)*body_mass*(body_link_x_dim*body_link_x_dim+body_link_z_dim*body_link_z_dim)}"
               iyz="0"
               izz="${(1/12)*body_mass*(body_link_x_dim*body_link_x_dim+body_link_y_dim*body_link_y_dim)}" />
    </inertial>
  </link>
  <!-- ###################################### -->
  <!-- END: BODY link of the robot    -->
  <!-- ###################################### -->

  <!-- ###################################### -->
  <!-- START: Back right wheel of the robot and the joint -->
  <!-- ###################################### -->
  <joint name="wheel1_joint" type="continuous">
    <parent link="body_link"/>
    <child link="wheel1_link"/>
    <origin xyz="${-body_link_x_dim/2+1.2*wheel_link_radius} ${-body_link_y_dim/2-wheel_link_length/2} ${wheel_link_z_location}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" velocity="1000"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>

  <link name="wheel1_link">
    <visual>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/>
      </geometry>
      <material name="yellow">
            <color rgba="1 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/>
      </geometry>
    </collision>
    <xacro:inertia_wheel/>
  </link>
  <!-- ###################################### -->
  <!-- END: Back right wheel of the robot and the joint -->
  <!-- ###################################### -->

  <!-- ###################################### -->
  <!-- START: Back left wheel of the robot and the joint -->
  <!-- ###################################### -->
  <joint name="wheel2_joint" type="continuous">
    <parent link="body_link"/>
    <child link="wheel2_link"/>
    <origin xyz="${-body_link_x_dim/2+1.2*wheel_link_radius} ${body_link_y_dim/2+wheel_link_length/2} ${wheel_link_z_location}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" velocity="1000"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>

  <link name="wheel2_link">
    <visual>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/>
      </geometry>
      <material name="yellow">
            <color rgba="1 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/>
      </geometry>
    </collision>
    <xacro:inertia_wheel/>
  </link>
  <!-- ###################################### -->
  <!-- END: Back left wheel of the robot and the joint -->
  <!-- ###################################### -->

  <!-- ###################################### -->
  <!-- START: Front right wheel of the robot and the joint -->
  <!-- ###################################### -->
  <joint name="wheel3_joint" type="continuous">
    <parent link="body_link"/>
    <child link="wheel3_link"/>
    <origin xyz="${body_link_x_dim/2-1.2*wheel_link_radius} ${-body_link_y_dim/2-wheel_link_length/2} ${wheel_link_z_location}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" velocity="1000"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>

  <link name="wheel3_link">
    <visual>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/>
      </geometry>
      <material name="yellow">
            <color rgba="1 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/>
      </geometry>
    </collision>
    <xacro:inertia_wheel/>
  </link>
  <!-- ###################################### -->
  <!-- END: Front right wheel of the robot and the joint -->
  <!-- ###################################### -->

  <!-- ###################################### -->
  <!-- START: Front left wheel of the robot and the joint -->
  <!-- ###################################### -->

  <joint name="wheel4_joint" type="continuous">
    <parent link="body_link"/>
    <child link="wheel4_link"/>
    <origin xyz="${body_link_x_dim/2-1.2*wheel_link_radius} ${body_link_y_dim/2+wheel_link_length/2} ${wheel_link_z_location}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000" velocity="1000"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>

  <link name="wheel4_link">
    <visual>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/>
      </geometry>
      <material name="yellow">
            <color rgba="1 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.570795 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/>
      </geometry>
    </collision>
    <xacro:inertia_wheel/>
  </link>
<!-- ###################################### -->
<!-- END: Front left wheel of the robot and the joint -->
<!-- ###################################### -->

</robot>

robot.gazebo

<?xml version="1.0" ?>
<!-- #################################################### -->
<!-- GAZEBO ADDITIONAL DESCRIPTION OF THE 4-WHEELED ROBOT -->
<!-- #################################################### -->
<robot>

  <!-- mu1 and mu2 are friction coefficients -->
  <gazebo reference="body_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Red</material>
  </gazebo>

  <gazebo reference="wheel1_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Yellow</material>
  </gazebo>

  <gazebo reference="wheel2_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Yellow</material>
  </gazebo>

  <gazebo reference="wheel3_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Yellow</material>
  </gazebo>

  <gazebo reference="wheel4_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Yellow</material>
  </gazebo>

  <!-- Controller for the 4-wheeled robot -->
  <gazebo>
    <plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
      <!-- Control update rate in Hz -->
      <updateRate>100.0</updateRate>
      <!-- Leave this empty otherwise, there will be problems with sending control commands -->
      <robotNamespace> </robotNamespace>
      <!-- Robot kinematics -->
      <leftFrontJoint>wheel4_joint</leftFrontJoint>
      <rightFrontJoint>wheel3_joint</rightFrontJoint>
      <leftRearJoint>wheel2_joint</leftRearJoint>
      <rightRearJoint>wheel1_joint</rightRearJoint>
      <wheelSeparation>${body_link_y_dim+wheel_link_length}</wheelSeparation>
      <wheelDiameter>${wheel_link_radius}</wheelDiameter>

      <!-- Maximum torque which the wheels can produce, in Nm, defaults to 5 Nm -->
      <torque>1000</torque>
      
      <!-- Topic to receive geometry_msgs/Twist message commands, defaults to 'cmd_vel' -->
      <commandTopic>cmd_vel</commandTopic>
      <!-- Topic to publish nav_msgs/Odometry messages, defaults to 'odom' -->
      <odometryTopic>odom</odometryTopic>
      <!-- Odometry frame, defaults to 'odom' -->
      <odometryFrame>odom</odometryFrame>
      <!-- Odometry Rate, defaults to 'odom' -->
      <odometryRate>20.0</odometryRate>
      <!-- Robot frame to calculate odometry from, defaults to 'base_footprint' -->
      <robotBaseFrame>dummy</robotBaseFrame>
      <!-- Set to true to publish transforms for the wheel links, defaults to false -->
      <publishWheelTF>true</publishWheelTF>
      <!-- Set to true to publish transforms for the wheel links, defaults to true -->
      <publishOdom>true</publishOdom>
      <!-- Set to true to publish sensor_msgs/JointState on /joint_state for the wheel joints, defaults to false -->
      <publishWheelJointState>true</publishWheelJointState>
      <!-- This part is required by Gazebo -->
      <covariance_x>0.0001</covariance_x>
      <covariance_y>0.0001</covariance_y>
      <covariance_yaw>0.01</covariance_yaw>
    </plugin>

  </gazebo>

</robot>

Thiết kế website giá rẻ

Danh mục

Simulating a Mecanum wheel robot in Gazebo