系统版本:Ubuntu14.04,ROS jade
机器人:小强
一、功能介绍
编写两个简单程序,一个是机器人直行,另一个是原地旋转
二、实验步骤
1、远程登录
ssh -X xiaoqiang@192.168.31.70
2、创建程序包
mkdir -p ~/Documents/mytest/src
cd ~/Documents/mytest/src
catkin_init_workspace
cd ~/Documents/mytest/
catkin_make
source devel/setup.bash
cd ~/Documents/mytest/src/
catkin_create_pkg robot_move roscpp geometry_msgs tf
3、 在robot_move/src/里创建goforward.cpp
cd robot_move/src/
vi goforward.cpp
并粘贴如下代码:
#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
ros::Publisher cmdVelPub;
void shutdown(int sig)
{
cmdVelPub.publish(geometry_msgs::Twist());//使机器人停止运动
ROS_INFO("goforward cpp ended!");
ros::shutdown();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "GoForward");//初始化ROS,它允许ROS通过命令行进行名称重映射
std::string topic = "/cmd_vel";
ros::NodeHandle node;//为这个进程的节点创建一个句柄
cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);//告诉master将要在/cmd_vel topic上发布一个geometry_msgs/Twist的消息
ros::Rate loopRate(10);//The desired rate to run at in Hz,ros::Rate对象可以允许你指定自循环的频率
// Override the default ros sigint handler. This must be set after the first NodeHandle is creat
signal(SIGINT, shutdown);
ROS_INFO("goforward cpp start...");
geometry_msgs::Twist speed; // 控制信号载体 Twist message
while (ros::ok())
{
speed.linear.x = 0.5; // 设置线速度为0.1m/s,正为前进,负为后退
speed.angular.z = 0; // 设置角速度为0rad/s,正为左转,负为右转
cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人
loopRate.sleep();//休眠直到一个频率周期的时间
}
return 0;
}
4、 在robot_move/src/里创建goincircles.cpp
vi goincircles.cpp
将goforward.cpp的源代码复制到goincircles.cpp,将speed.linear.x设置为0,speed.angular.z设置为0.5,其它不变。
5、在robot_move/src/里创建odom_out_and_back.cpp
#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
ros::Publisher cmdVelPub;
void shutdown(int sig)
{
cmdVelPub.publish(geometry_msgs::Twist());
ROS_INFO("odom_out_and_back.cpp ended!");
ros::shutdown();
}
int main(int argc, char** argv)
{
//How fast will we update the robot's movement?
double rate = 20;
int count = 0;//Loop through the two legs of the trip
ros::init(argc, argv, "go_and_back");
std::string topic = "/cmd_vel";
ros::NodeHandle node;
cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);
//Set the equivalent ROS rate variable
ros::Rate loopRate(rate);
geometry_msgs::Twist speed; // 控制信号载体 Twist message
signal(SIGINT, shutdown);
ROS_INFO("odom_out_and_back.cpp start...");
//Set the forward linear speed to 0.2 meters per second
float linear_speed = 0.2;
//Set the travel distance to 1.0 meters
float goal_distance = 1.0;
//Set the rotation speed to 0.5 radians per second
float angular_speed = 0.5;
//Set the rotation angle to Pi radians (180 degrees)
double goal_angle = M_PI;
//Set the angular tolerance in degrees converted to radians
double angular_tolerance = 2.5*M_PI/180; //角度转换成弧度:deg*PI/180
tf::TransformListener listener;
tf::StampedTransform transform;
//Find out if the robot uses /base_link or /base_footprint
std::string odom_frame = "/odom";
std::string base_frame;
try
{
listener.waitForTransform(odom_frame, "/base_footprint", ros::Time(), ros::Duration(2.0) );
base_frame = "/base_footprint";
ROS_INFO("base_frame = /base_footprint");
}
catch (tf::TransformException & ex)
{
try
{
listener.waitForTransform(odom_frame, "/base_link", ros::Time(), ros::Duration(2.0) );
base_frame = "/base_link";
ROS_INFO("base_frame = /base_link");
}
catch (tf::TransformException ex)
{
ROS_INFO("Cannot find transform between /odom and /base_link or /base_footprint");
cmdVelPub.publish(geometry_msgs::Twist());
ros::shutdown();
}
}
//Loop once for each leg of the trip
for(int i = 0;i < 2;i++)
{
ROS_INFO("go straight...!");
speed.linear.x = linear_speed; // 设置线速度,正为前进,负为后退
//Get the starting position values
listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
float x_start = transform.getOrigin().x();
float y_start = transform.getOrigin().y();
// Keep track of the distance traveled
float distance = 0;
while( (distance < goal_distance) && (ros::ok()) )
{
//Publish the Twist message and sleep 1 cycle
cmdVelPub.publish(speed);
loopRate.sleep();
listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
//Get the current position
float x = transform.getOrigin().x();
float y = transform.getOrigin().y();
//Compute the Euclidean distance from the start
distance = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));
}
//Stop the robot before the rotation
cmdVelPub.publish(geometry_msgs::Twist());
ros::Duration(1).sleep(); // sleep for a second
ROS_INFO("rotation...!");
//Now rotate left roughly 180 degrees
speed.linear.x = 0;
//Set the angular speed
speed.angular.z = angular_speed; // 设置角速度,正为左转,负为右转
//yaw是围绕Y轴旋转,也叫偏航角
//Track the last angle measured
double last_angle = fabs(tf::getYaw(transform.getRotation()));
//Track how far we have turned
double turn_angle = 0;
while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) )
{
//Publish the Twist message and sleep 1 cycle
cmdVelPub.publish(speed);
loopRate.sleep();
// Get the current rotation
listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
//C++: abs()求得是正数的绝对值,fabs()求得是浮点数的绝对值;python:abs(x),参数可以是:负数、正数、浮点数或者长整形
double rotation = fabs(tf::getYaw(transform.getRotation()));
//Compute the amount of rotation since the last loop
double delta_angle = fabs(rotation - last_angle);
//Add to the running total
turn_angle += delta_angle;
last_angle = rotation;
}
//Stop the robot before the rotation
//Set the angular speed
speed.angular.z = 0;
cmdVelPub.publish(geometry_msgs::Twist());
ros::Duration(1).sleep(); // sleep for a second
}
cmdVelPub.publish(geometry_msgs::Twist());
ros::Duration(1).sleep(); // sleep for a second
ROS_INFO("odom_out_and_back.cpp ended!");
ros::shutdown();
return 0;
}
6、 修改robot_move目录下的CMakeLists.txt
cd ..
vi CMakeLists.txt
将catkin_package()中的CATKIN_DEPENDS geometry_msgs roscpp去注释
在CMakeLists.txt文件末尾加入几条语句:
add_executable(goforward src/goforward.cpp)
target_link_libraries(goforward ${catkin_LIBRARIES})
add_executable(goincircles src/goincircles.cpp)
target_link_libraries(goincircles ${catkin_LIBRARIES})
add_executable(odom_out_and_back src/odom_out_and_back.cpp)
target_link_libraries(odom_out_and_back ${catkin_LIBRARIES})
7、 编译程序
在mytest目录下
catkin_make
8、 测试程序
8.1 启动goforward程序 source devel/setup.bash
rosrun robot_move goforward
8.2 Ctrl+C结束goforward程序,再测试goincircles程序
rosrun robot_move goincircles
8.3 Ctrl+C结束goincircles程序,再测试odom_out_and_back程序
rosrun robot_move odom_out_and_back