（轉(zhuǎn)載請(qǐng)注明作者和出處：https://yangningbocn.github.io 未經(jīng)允許請(qǐng)勿用于商業(yè)用途）
本博客只是個(gè)人學(xué)習(xí)記錄使用，瞎寫(xiě)寫(xiě)，內(nèi)容比較粗糙。編寫(xiě)過(guò)程中，借鑒了古月居大神的smartcar和ROS-WIKI以及其它一些大神的內(nèi)容。
引入古月居大神的鏈接使用smartcar進(jìn)行仿真和smartcar源碼上傳
下面我針對(duì)自己的機(jī)器人編寫(xiě)的代碼和文件。
launch文件
<launch>
<param name="/use_sim_time" value="false" />
<!-- Load the URDF/Xacro model of our robot -->
<arg name="urdf_file" default="$(find xacro)/xacro.py '$(find smartcar_description)/urdf/smartcar.urdf.xacro'" />
<arg name="gui" default="true" />
<param name="robot_description" command="$(arg urdf_file)" />
<param name="use_gui" value="$(arg gui)"/>
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find smartcar_description)/config/smartcar_arbotix.yaml" command="load" />
<param name="sim" value="true"/>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
<param name="publish_frequency" type="double" value="20.0" />
</node>
<!-- We need a static transforms for the wheels -->
<node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /left_front_link 100" />
<node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /right_front_link 100" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/urdf.rviz" />
</launch>
xacro文件包括三部分：
- smartcar_body
<?xml version="1.0"?>
<robot name="smartcar" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="M_PI" value="3.14159"/>
<!-- Macro for SmartCar body. Including Gazebo extensions, but does not include Kinect -->
<xacro:include filename="$(find smartcar_description)/urdf/gazebo.urdf.xacro"/>
<xacro:property name="base_x" value="0.33" />
<xacro:property name="base_y" value="0.33" />
<xacro:macro name="smartcar_body">
<link name="base_footprint">
<inertial>
<mass value="0.0001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
<link
name="base_link">
<inertial>
<origin
xyz="0.0032273 -0.060593 1.3595E-05"
rpy="0 0 0" />
<mass
value="4.2623" />
<inertia
ixx="0.04568"
ixy="-0.00094545"
ixz="-8.9814E-06"
iyy="0.11402"
iyz="5.3399E-06"
izz="0.11035" />
</inertial>
<visual>
<origin
xyz="0 0 0.23"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0.23"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="left_front_wheel">
<inertial>
<origin
xyz="0 -1.98217916108356E-05 0"
rpy="0 0 0" />
<mass
value="0.176438810476987" />
<inertia
ixx="8.8965933887671E-05"
ixy="-1.27636183461175E-23"
ixz="8.0831821149659E-23"
iyy="8.89898462049031E-05"
iyz="2.16519665002917E-23"
izz="0.000130905430632044" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/left_front_wheel.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 0 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/left_front_wheel.STL" />
</geometry>
</collision>
</link>
<joint
name="left_front_wheel_joint"
type="continuous">
<origin
xyz="0.12875 0.1504 0.04"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="left_front_wheel" />
<axis
xyz="0 0 1" />
</joint>
<link
name="right_front_wheel">
<inertial>
<origin
xyz="0 -1.98217916108079E-05 0"
rpy="0 0 0" />
<mass
value="0.176438810476987" />
<inertia
ixx="8.8965933887671E-05"
ixy="-1.27636183461175E-23"
ixz="8.08318211496588E-23"
iyy="8.89898462049032E-05"
iyz="2.18596653030412E-23"
izz="0.000130905430632044" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/right_front_wheel.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 0 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/right_front_wheel.STL" />
</geometry>
</collision>
</link>
<joint
name="right_front_wheel_joint"
type="continuous">
<origin
xyz="0.12875 -0.1504 0.04"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="right_front_wheel" />
<axis
xyz="0 0 1" />
</joint>
<link
name="left_back_wheel">
<inertial>
<origin
xyz="0 -1.98217916108356E-05 0"
rpy="0 0 0" />
<mass
value="0.176438810476987" />
<inertia
ixx="8.8965933887671E-05"
ixy="-1.27300145019925E-23"
ixz="8.0831821149659E-23"
iyy="8.89898462049032E-05"
iyz="2.16519665002917E-23"
izz="0.000130905430632044" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/left_back_wheel.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 0 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/left_back_wheel.STL" />
</geometry>
</collision>
</link>
<joint
name="left_back_wheel_joint"
type="continuous">
<origin
xyz="-0.12875 0.1504 0.04"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="left_back_wheel" />
<axis
xyz="0 0 1" />
</joint>
<link
name="right_back_wheel">
<inertial>
<origin
xyz="2.77555756156289E-17 -1.98217916108079E-05 0"
rpy="0 0 0" />
<mass
value="0.176438810476987" />
<inertia
ixx="8.8965933887671E-05"
ixy="-4.17259369791981E-24"
ixz="2.75455648073554E-21"
iyy="8.89898462049032E-05"
iyz="5.21854052435048E-20"
izz="0.000130905430632044" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/right_back_wheel.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 0 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://smartcar_description/meshes/right_back_wheel.STL" />
</geometry>
</collision>
</link>
<joint
name="right_back_wheel_joint"
type="continuous">
<origin
xyz="-0.12875 -0.1504 0.04"
rpy="1.5708 0 3.1416" />
<parent
link="base_link" />
<child
link="right_back_wheel" />
<axis
xyz="0 0 1" />
</joint>
</xacro:macro>
</robot>
- gazebo.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:xacro="http://ros.org/wiki/xacro"
name="smartcar_gazebo">
<!-- ASUS Xtion PRO camera for simulation -->
<!-- gazebo_ros_wge100 plugin is in kt2_gazebo_plugins package -->
<xacro:macro name="smartcar_sim">
<gazebo reference="base_footprint">
<material>Gazebo/Blue</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="right_front_wheel">
<material>Gazebo/FlatBlack</material>
</gazebo>
<transmission name="right_front_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="right_front_wheel_joint" />
<actuator name="right_front_wheel_joint_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo reference="right_back_wheel">
<material>Gazebo/FlatBlack</material>
</gazebo>
<transmission name="right_back_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="right_back_wheel_joint" />
<actuator name="right_back_wheel_joint_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo reference="left_front_wheel">
<material>Gazebo/FlatBlack</material>
</gazebo>
<transmission name="left_front_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="left_front_wheel_joint" />
<actuator name="left_front_wheel_joint_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo reference="left_back_wheel">
<material>Gazebo/FlatBlack</material>
</gazebo>
<transmission name="left_back_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="left_back_wheel_joint" />
<actuator name="left_back_wheel_joint_motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- position controller -->
<gazebo>
<plugin name="base_controller" filename="libgazebo_ros_planar_move.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<odometryRate>100.0</odometryRate>
<robotBaseFrame>base_footprint</robotBaseFrame>
</plugin>
</gazebo>
</xacro:macro>
</robot>
- smartcar.urdf.xacro
<?xml version="1.0"?>
<robot name="smartcar"
xmlns:xi="http://www.w3.org/2001/XInclude"
xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find smartcar_description)/urdf/smartcar_body.urdf.xacro" />
<!-- Body of SmartCar, with plates, standoffs and Create (including sim sensors) -->
<smartcar_body/>
<smartcar_sim/>
</robot>
然后按照古月大神的方法啟動(dòng)并控制就可以了。下面我要說(shuō)的就是我比較獨(dú)特的部分了。
現(xiàn)在大部分的移動(dòng)機(jī)器人主要差動(dòng)輪進(jìn)行運(yùn)動(dòng)，只有l(wèi)inear.x和angular.z兩個(gè)值，但是我的模型是４個(gè)瑞典輪，能提供橫向的移動(dòng)速度，即linear.y。這里對(duì)diff_controller進(jìn)行修改讓他能夠接受linear.y。
diff_controller.py在/opt/ros/indigo/lib/python2.7/dist-packages下。（可以用sudo vim進(jìn)行修改），修改都已經(jīng)注釋過(guò)了（add by yang）。代碼如下：
#!/usr/bin/env python
"""
diff_controller.py - controller for a differential drive
Copyright (c) 2010-2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
from math import sin,cos,pi
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from diagnostic_msgs.msg import *
from tf.broadcaster import TransformBroadcaster
from ax12 import *
from controllers import *
from struct import unpack
class DiffController(Controller):
""" Controller to handle movement & odometry feedback for a differential
drive mobile base. """
def __init__(self, device, name):
Controller.__init__(self, device, name)
self.pause = True
self.last_cmd = rospy.Time.now()
# parameters: rates and geometry
self.rate = rospy.get_param('~controllers/'+name+'/rate',10.0)
self.timeout = rospy.get_param('~controllers/'+name+'/timeout',1.0)
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
self.ticks_meter = float(rospy.get_param('~controllers/'+name+'/ticks_meter'))
self.base_width = float(rospy.get_param('~controllers/'+name+'/base_width'))
self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', 'base_link')
self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom')
# parameters: PID
self.Kp = rospy.get_param('~controllers/'+name+'/Kp', 5)
self.Kd = rospy.get_param('~controllers/'+name+'/Kd', 1)
self.Ki = rospy.get_param('~controllers/'+name+'/Ki', 0)
self.Ko = rospy.get_param('~controllers/'+name+'/Ko', 50)
# parameters: acceleration
self.accel_limit = rospy.get_param('~controllers/'+name+'/accel_limit', 0.1)
self.max_accel = int(self.accel_limit*self.ticks_meter/self.rate)
# output for joint states publisher
self.joint_names = ["base_l_wheel_joint","base_r_wheel_joint"]
self.joint_positions = [0,0]
self.joint_velocities = [0,0]
# internal data
self.v_left = 0 # current setpoint velocity
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dy = 0 # add by yang
self.dr = 0
self.then = rospy.Time.now() # time for determining dx/dy
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started yang DiffController ("+name+"). Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.")
def startup(self):
if not self.fake:
self.setup(self.Kp,self.Kd,self.Ki,self.Ko)
def update(self):
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
if self.fake:
x = cos(self.th)*self.dx*elapsed
y = -sin(self.th)*self.dx*elapsed
#self.x and self.y has changed for the effort of dy
self.x += cos(self.th)*self.dx*elapsed + sin(self.th)*self.dy*elapsed
self.y += sin(self.th)*self.dx*elapsed + cos(self.th)*self.dy*elapsed
self.th += self.dr*elapsed
else:
# read encoders
try:
left, right = self.status()
except Exception as e:
rospy.logerr("Could not update encoders: " + str(e))
return
rospy.logdebug("Encoders: " + str(left) +","+ str(right))
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (left - self.enc_left)/self.ticks_meter
d_right = (right - self.enc_right)/self.ticks_meter
self.enc_left = left
self.enc_right = right
d = (d_left+d_right)/2
th = (d_right-d_left)/self.base_width
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
x = cos(th)*d
y = -sin(th)*d
self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
self.y = self.y + (sin(self.th)*x + cos(self.th)*y)
if (th != 0):
self.th = self.th + th
# publish or perish
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th/2)
quaternion.w = cos(self.th/2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = self.dy #it was 0 before changed
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
if now > (self.last_cmd + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
# update motors
if not self.fake:
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.write(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def shutdown(self):
if not self.fake:
self.write(0,0)
def cmdVelCb(self,req):
""" Handle movement requests. """
self.last_cmd = rospy.Time.now()
if self.fake:
self.dx = req.linear.x # m/s
self.dy = req.linear.y # m/s add by yang
self.dr = req.angular.z # rad/s
else:
# set motor speeds in ticks per 1/30s
self.v_des_left = int( ((req.linear.x - (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
self.v_des_right = int( ((req.linear.x + (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
def getDiagnostics(self):
""" Get a diagnostics status. """
msg = DiagnosticStatus()
msg.name = self.name
msg.level = DiagnosticStatus.OK
msg.message = "OK"
if not self.fake:
msg.values.append(KeyValue("Left", str(self.enc_left)))
msg.values.append(KeyValue("Right", str(self.enc_right)))
msg.values.append(KeyValue("dX", str(self.dx)))
msg.values.append(KeyValue("dR", str(self.dr)))
return msg
###
### Controller Specification:
###
### setup: Kp, Kd, Ki, Ko (all unsigned char)
###
### write: left_speed, right_speed (2-byte signed, ticks per frame)
###
### status: left_enc, right_enc (4-byte signed)
###
def setup(self, kp, kd, ki, ko):
success = self.device.execute(253, AX_CONTROL_SETUP, [10, kp, kd, ki, ko])
def write(self, left, right):
""" Send a closed-loop speed. Base PID loop runs at 30Hz, these values
are therefore in ticks per 1/30 second. """
left = left&0xffff
right = right&0xffff
success = self.device.execute(253, AX_CONTROL_WRITE, [10, left%256, left>>8, right%256, right>>8])
def status(self):
""" read 32-bit (signed) encoder values. """
values = self.device.execute(253, AX_CONTROL_STAT, [10])
left_values = "".join([chr(k) for k in values[0:4] ])
right_values = "".join([chr(k) for k in values[4:] ])
try:
left = unpack('=l',left_values)[0]
right = unpack('=l',right_values)[0]
return [left, right]
except:
return None
