这是一个为了检验ros学习成果的简单小程序。因为博主比较菜-_-|||,这个程序遇到了很多问题,特此写个博客记录一下。
程序实现的功能利用urdf制作一个简易小车,并使用键盘控制,当然不管是制作小车还是让小车动起来都很简单,主要是键盘控制难以实现,为了简单起见,我使用了turtlesim中的turtlesim_node节点和turtle_teleop_key节点,如果没有下载turtlesim包请尝试以下指令
sudo apt-get install ros-indigo-turtlesim
接下来我们来看一下这两个节点的信息:
首先是turtlesim_node节点
Node [/turtle_node]
Publications:
* /turtle1/color_sensor [turtlesim/Color]
* /rosout [rosgraph_msgs/Log]
* /turtle1/pose [turtlesim/Pose]
Subscriptions:
* /turtle1/cmd_vel [geometry_msgs/Twist]
从这里我们可以看到这个节点会发布一个/turtle1/pose的主题,可以简单的想到我们只要通过订阅这个主题就可以实现通过乌龟的位置控制小车的位置,再运行一下turtle_teleop_key节点就万事大吉了
然后就是订阅了以后在发送信息给tf的问题了,这里稍微废了点了力气才搞出来。
代码如下
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <sensor_msgs/JointState.h>
void Callback(const turtlesim::Pose& msg)
{
static tf::TransformBroadcaster broadcaster;
//通过这个发布小车与odom的位置变化
ros::Rate loop_rate(30);
tf::Transform transform;
transform.setOrigin(tf::Vector3(msg.x,msg.y,0.0));
tf::Quaternion q;
q.setRPY(0,0, msg.theta);
transform.setRotation(q);
broadcaster.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"odom","base_link"));
/*geometry_msgs::TransformStamped odom_trans;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
//header.frame设置为odom
odom_trans.header.stamp = ros::Time::now();
odom_trans.transform.translation.x = msg.y;
odom_trans.transform.translation.y = msg.x;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation.z = 1.0;
odom_trans.transform.rotation.w = msg.theta;
broadcaster.sendTransform(odom_trans);
*/
}
int main(int argc , char** argv)
{
ros::init(argc , argv , "robot_car_listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("turtle1/pose",10,&Callback);
//订阅cmd_vel主题
ros::spin();
return 0;
}