启动语句
roslaunch turtlebot3_fake turtlebot3_fake.launch //启动rviz、话题通信
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch //启动键盘控制节点
turtlebot3_fake.launch文件解读
<launch>
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
<param name="tb3_model" value="$(arg model)"/>
<include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
<arg name="model" value="$(arg model)" />
</include>
<node pkg="turtlebot3_fake" type="turtlebot3_fake_node" name="turtlebot3_fake_node" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot3_fake)/rviz/turtlebot3_fake.rviz"/>
</launch>
这个launch文件包含三部分:加载机器人模型、启动节点话题、启动rviz。
其中,turtlebot3_fake_node是自定义的一个节点。代码如下:
1. 主函数
int main(int argc, char* argv[])
{
ros::init(argc, argv, "turtlebot3_fake_node");
Turtlebot3Fake tb3fake;
ros::Rate loop_rate(30);
while (ros::ok())
{
tb3fake.update();
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
定义turtlebot3_fake_node这个节点
2. Turtlebot3Fake::init()
发布burger的关节状态信息和里程计信息,订阅速度信息
joint_states_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 100);
odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 100);
cmd_vel_sub_ = nh_.subscribe("cmd_vel", 100, &Turtlebot3Fake::commandVelocityCallback, this);
void Turtlebot3Fake::commandVelocityCallback(const geometry_msgs::TwistConstPtr cmd_vel_msg)
{
last_cmd_vel_time_ = ros::Time::now();
goal_linear_velocity_ = cmd_vel_msg->linear.x;
goal_angular_velocity_ = cmd_vel_msg->angular.z;
wheel_speed_cmd_[LEFT] = goal_linear_velocity_ - (goal_angular_velocity_ * wheel_seperation_ / 2);
wheel_speed_cmd_[RIGHT] = goal_linear_velocity_ + (goal_angular_velocity_ * wheel_seperation_ / 2);
}
订阅得到的速度、角速度定义为全局速度、全局角速度。通过全局速度以及全局角速度得到左右轮的速度。
3. tb3fake.update()
bool Turtlebot3Fake::update()
{
ros::Time time_now = ros::Time::now();
ros::Duration step_time = time_now - prev_update_time_;
prev_update_time_ = time_now;
if((time_now - last_cmd_vel_time_).toSec() > cmd_vel_timeout_)
{
wheel_speed_cmd_[LEFT] = 0.0;
wheel_speed_cmd_[RIGHT] = 0.0;
}
updateOdometry(step_time);
odom_.header.stamp = time_now;
odom_pub_.publish(odom_);
updateJoint();
joint_states_.header.stamp = time_now;
joint_states_pub_.publish(joint_states_);
geometry_msgs::TransformStamped odom_tf;
updateTF(odom_tf);
tf_broadcaster_.sendTransform(odom_tf);
return true;
}
4. Turtlebot3Fake::updateOdometry(ros::Duration diff_time)
通过订阅得到的速度信息,更新里程计信息,包括位置、速度、角速度。通过update()函数将更新过的里程计信息发布出去。
bool Turtlebot3Fake::updateOdometry(ros::Duration diff_time)
{
double wheel_l, wheel_r;
double delta_s, delta_theta;
double v[2], w[2];
wheel_l = wheel_r = 0.0;
delta_s = delta_theta = 0.0;
v[LEFT] = wheel_speed_cmd_[LEFT];
w[LEFT] = v[LEFT] / WHEEL_RADIUS;
v[RIGHT] = wheel_speed_cmd_[RIGHT];
w[RIGHT] = v[RIGHT] / WHEEL_RADIUS;
last_velocity_[LEFT] = w[LEFT];
last_velocity_[RIGHT] = w[RIGHT];
wheel_l = w[LEFT] * diff_time.toSec();
wheel_r = w[RIGHT] * diff_time.toSec();
if(isnan(wheel_l))
{
wheel_l = 0.0;
}
if(isnan(wheel_r))
{
wheel_r = 0.0;
}
last_position_[LEFT] += wheel_l;
last_position_[RIGHT] += wheel_r;
delta_s = WHEEL_RADIUS * (wheel_r + wheel_l) / 2.0;
delta_theta = WHEEL_RADIUS * (wheel_r - wheel_l) / wheel_seperation_;
odom_pose_[0] += delta_s * cos(odom_pose_[2] + (delta_theta / 2.0));
odom_pose_[1] += delta_s * sin(odom_pose_[2] + (delta_theta / 2.0));
odom_pose_[2] += delta_theta;
odom_vel_[0] = delta_s / diff_time.toSec();
odom_vel_[1] = 0.0;
odom_vel_[2] = delta_theta / diff_time.toSec();
odom_.pose.pose.position.x = odom_pose_[0];
odom_.pose.pose.position.y = odom_pose_[1];
odom_.pose.pose.position.z = 0;
odom_.pose.pose.orientation = tf::createQuaternionMsgFromYaw(odom_pose_[2]);
odom_.twist.twist.linear.x = odom_vel_[0];
odom_.twist.twist.angular.z = odom_vel_[2];
return true;
}
启动rviz
首先打开一个空的rviz,Add需要的属性,save as 文件名为turtelbot3_fake.rviz。此时,会自动生成文件名为turtlebot3_fake.rviz的文件。
另一种turtlebot3_fake.launch写法
<launch>
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="x_pos" default="0.0"/>
<arg name="y_pos" default="0.0"/>
<arg name="z_pos" default="0.0"/>
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
<node pkg="turtlebot3_fake" type="turtlebot3_fake_node" name="turtlebot3_fake_node" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot3_fake)/rviz/turtlebot3_fake.rviz"/>
</launch>
这种写法与开头的.launch文件的不同之处在于加载burger的方式不同。
键盘控制代码见下篇。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)