沉默杀手HomeArchivesTagsCategories
ROS编程常用函数
|ROSROS程序和源码分析|
Word count: 1.9k|Reading time: 9 min

接受中文注释

setlocale(LC_ALL, "");

ros::Time 和 double 转换

1
2
double x=ros::Time::now().toSec();     //把时间戳转化成浮点型格式
ros::Time stamp = ros::Time().fromSec(x);    //把浮点型变成时间戳

读取环境变量

跨平台函数 bool get_environment_variable (std::string &str, const char *environment_variable)

clamp函数

1
2
3
4
5
6
7
8
9
template<class T>
T clamp(T x, T min, T max)
{
    if (x > max)
        return max;
    if (x < min)
        return min;
    return x;
}
1
2
3
4
// 注意最后一个路径点，是 end() 的前一个点
geometry_msgs::PoseStamped  global_end = *(std::prev(global_plan_.end()) );
geometry_msgs::PoseStamped  base_end;
transformPose("base_link", global_end,  base_end);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
bool  transformPose(
    const std::string frame,
    geometry_msgs::PoseStamped &in_pose,
    geometry_msgs::PoseStamped &out_pose) const
{
    if (in_pose.header.frame_id == frame)
    {
        out_pose = in_pose;
        return true;
    }
    try
    {
        out_pose.header.frame_id = frame;
        tf_->transformPose(frame, in_pose, out_pose);
        out_pose.header.seq = in_pose.header.seq;
        return true;
    }
    catch (tf2::TransformException &ex)
    {
        ROS_ERROR("Exception in transformPose : %s", ex.what()  );
    }
    return false;
}

四元数转欧拉角yaw

1
2
3
4
5
6
7
8
9
10
11
12
13
14
double quaternionToYaw(const geometry_msgs::Quaternion&  quat)
{
    tf::Quaternion tf_quat;
    tf::quaternionMsgToTF(quat, tf_quat);
    double roll, pitch, yaw;
    tf::Matrix3x3(tf_quat).getRPY(roll, pitch, yaw);
    return yaw;
}

double quaternionToYaw(const geometry_msgs::Quaternion&  quat)
{
	tf::Quaternion quat(0,0, quat.z, quat.w);
	return tf::getYaw(quat);
}

欧拉角 —— 四元数

1
2
3
4
5
6
7
8
9
//只通过yaw计算四元数，用于平面小车
static geometry_msgs::Quaternion  tf::createQuaternionMsgFromYaw(double yaw);

static tf::Quaternion  createQuaternionFromYaw (double yaw)

static tf::Quaternion  createQuaternionFromRPY(double roll, double pitch, double yaw)

static geometry_msgs::Quaternion  createQuaternionMsgFromRollPitchYaw( double roll, 
    double pitch,   double yaw)

获得当前节点名称

ros::this_node::getName()

bool ros::master::getNodes (V_string &nodes) 是获得当前所有运行的节点，效果和rosnode list一样。但这里有个缺陷，使用pkill杀死的节点，在rosnode list里仍然能看到，实际已经退出了。

规范角度 [-π, π)

调用g2o::normalize_theta(double theta)或者

1
2
3
4
5
6
7
void normAngle ( double& angle )
{
    if( angle >= M_PI)
        angle -= 2 * M_PI;
    if( angle < -M_PI)
        angle += 2 * M_PI;
}

判断机器人是否静止

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
bool robotIsStill()
{
    bool velStatus;
    boost::shared_ptr<nav_msgs::Odometry const> twistEdge;
    twistEdge = ros::topic::waitForMessage<nav_msgs::Odometry>("odom",ros::Duration(0.3) );
    if(twistEdge != NULL)
    {
        double velX = twistEdge->twist.twist.linear.x;
        double angularZ = twistEdge->twist.twist.angular.z;
        if(velX == 0 && angularZ == 0)
            velStatus = true;
        else
            velStatus = false;
    }
    else velStatus = false;
    return velStatus;
}

移动一定时间，也可以表示移动一定距离

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = -0.12;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;

auto t1 = std::chrono::steady_clock::now();
auto t2 = std::chrono::steady_clock::now();
ROS_INFO(" -------------- backward begin ! ------------------ ");
while( std::chrono::duration<double>(t2-t1).count() < 3 )
{
    vel_pub.publish(cmd_vel);
    t2 = std::chrono::steady_clock::now();
}
cmd_vel.linear.x = 0;
vel_pub.publish(cmd_vel);
ROS_INFO(" -------------- backward done, now continue !  ------------------ ");

获取当前速度

1
2
3
4
5
6
7
8
tf::Stamped<tf::Pose> robot_vel_tf;
base_local_planner::OdometryHelperRos odom_helper_;
odom_helper_.getRobotVel(robot_vel_tf);

geometry_msgs::Twist  robot_vel_;
robot_vel_.linear.x = robot_vel_tf.getOrigin().getX();
robot_vel_.linear.y = robot_vel_tf.getOrigin().getY();
robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation());

poseStampedMsgToTF

1
2
3
4
5
6
7
8
//get the starting pose of the robot
tf::Stamped<tf::Pose> global_pose;
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
  ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
  return false;
}
geometry_msgs::PoseStamped start_pose;
tf::poseStampedTFToMsg(global_pose, start_pose);

还有类似的函数: poseTFToMsg 和 transformMsgToTF

angles包

  • static double from_degrees (double degrees): Convert degrees to radians

  • static double normalize_angle (double angle)

  • static double shortest_angular_distance (double from, double to)

  • static double to_degrees (double radians): Convert radians to degrees

  • static double two_pi_complement (double angle): returns the angle in [-2*M_PI, 2*M_PI] going the other way along the unit circl

tf 命名空间

tf的命名空间有很多有用的函数，先列出一部分:

  • TFSIMD_FORCE_INLINE tfScalar angle (const Quaternion &q1, const Quaternion &q2)

Return the half angle between two quaternions

  • TFSIMD_FORCE_INLINE tfScalar angleShortestPath (const Quaternion &q1, const Quaternion &q2)

Return the shortest angle between two quaternions

  • void assertQuaternionValid (const geometry_msgs::Quaternion &q)

Throw InvalidArgument if quaternion is malformed

  • void assertQuaternionValid (const tf::Quaternion &q)

Throw InvalidArgument if quaternion is malformed

  • TFSIMD_FORCE_INLINE Quaternion shortestArcQuat (const Vector3 &v0, const Vector3 &v1)

TFSIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2 (Vector3 &v0, Vector3 &v1)

  • TFSIMD_FORCE_INLINE Quaternion slerp (const Quaternion &q1, const Quaternion &q2, const tfScalar &t)

Return the result of spherical linear interpolation betwen two quaternions

  • std::string strip_leading_slash (const std::string &frame_name)

  • TFSIMD_FORCE_INLINE tfScalar tfAngle (const Vector3 &v1, const Vector3 &v2)

Return the angle between two vectors

  • TFSIMD_FORCE_INLINE Vector3 tfCross (const Vector3 &v1, const Vector3 &v2)

Return the cross product of two vectors

  • TFSIMD_FORCE_INLINE tfScalar tfDistance (const Vector3 &v1, const Vector3 &v2)

Return the distance between two vectors

  • TFSIMD_FORCE_INLINE tfScalar tfDistance2 (const Vector3 &v1, const Vector3 &v2)

Return the distance squared between two vectors

  • TFSIMD_FORCE_INLINE tfScalar tfDot (const Vector3 &v1, const Vector3 &v2)

Return the dot product between two vectors

goalToGlobalFrame

geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg)
这个函数的核心功能是获得机器人坐标系和全局地图坐标系的关系：

1
tf_.transformPose(global_frame, goal_pose, global_pose);

计算机器人的内接圆和外切圆半径

1
2
3
4
5
std::vector<geometry_msgs::Point> footprint = planner_costmap_ros_->getRobotFootprint();
costmap_2d::calculateMinAndMaxDistances(footprint, robot_inscribed_radius_, robot_circumscribed_radius);
// 或者这样
planner_costmap_ros_->getLayeredCostmap()->getInscribedRadius()
planner_costmap_ros_->getLayeredCostmap()->getCircumscribedRadius()

通过footprint的代价值判断是否撞障碍

与上一条结合，参考TEB代码

1
2


计算 直角三角形的斜边长

#include <math.h>

double hypot(double x, double y)，输入是两个直角边，用法: hypot(p1.x - p2.x, p1.y - p2.y)

ros::shutdown()

关闭节点并且从主节点登出，会终结所有的订阅、发布、服务，连ROS_INFO也不能再用。一般不用调用此函数，当所有的NodeHandle销毁时，就会自动关闭节点。默认的roscpp也会安装SIGINT句柄用来检测Ctrl-C，并自动为你关闭节点。

ros::waitForShutdown ()

等待节点关闭，无论时通过Ctrl-C还是ros::shutdown()

ros::isShuttingDown()

一旦ros::shutdown()调用（注意是刚开始调用，而不是调用完毕）就返回true。一般建议用ros::ok()，特殊情况可以用ros::isShuttingDown()

ros::NodeHandle

获取节点的句柄，ros::init是初始化节点，这个是启动节点。
如果不想通过对象的生命周期来管理节点的开始和结束，可以通过ros::start()和ros::shutdown()来自己管理节点。

ros::ok()

判断是否退出节点，如果返回false，说明可能发生了以下事件

  • 调用了ros::shutdown()
  • 被另一同名节点踢出ROS网络
  • 节点中的所有ros::NodeHandles都已经被销毁

ros::Time::now()

获取当前时间: ros::Time time = ros::Time::now()
使用模拟时间时，当 /clock节点接受到第一条消息时，ros::Time::now()返回时刻 0，此时客户端还不知道时钟时间。

ros::Rate

1
2
3
4
5
6
ros::Rate rate(10);
while(ros::ok())
{
    // do something
    rate.sleep();
}

Rate类的构造函数参数是频率，不是时间。rate(10)对应时间间隔为1/10秒，也就是100毫秒。Rate::sleep()是进行睡眠，实际调用Duration::sleep()，间隔就是从构造函数开始计算，源码处理比较复杂。

获得当前运行的所有节点名

1
2
3
typedef std::vector<std::string> ros::V_string
// Retreives the currently-known list of nodes from the master. 放入
ROSCPP_DECL bool   getNodes (V_string &nodes)

用这个函数很简单：

1
2
3
4
5
6
7
std::vector<std::string> nodes;
ros::master::getNodes(nodes);	// bool
int len = nodes.size();
for (int i =0; i < len; i ++)
{
	cout<<"node: "<<nodes.at(i)<<endl;
}

获得当前运行的所有话题名

1
2
3
4
5
6
7
8
9
10
11
struct ros::master::TopicInfo
{
	TopicInfo ()
 	TopicInfo (const std::string &_name, const std::string &_datatype)
public:
	std::string 	datatype    // Datatype of the topic. 
	std::string 	name        // Name of the topic. 
}
typedef std::vector<TopicInfo> ros::master::V_TopicInfo

ROSCPP_DECL bool    getTopics (V_TopicInfo &topics)

使用是类似的：

1
2
3
4
5
6
7
8
std::vector<ros::master::TopicInfo> topics;
ros::master::getTopics(topics);
int len = topics.size();
for (int i =0; i < len; i ++)
{
	cout<<"topic: "<<topics.at(i).name<<"   type: "
		<<topics.at(i).datatype<<endl;
}

其他可能用到的函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
ros::isInitialized ()    // 判断ros::init()是否被调用

ros::isStarted ()	　// 判断节点是否已经通过ros::start()启动

ros::requestShutdown ()　　// 在节点的一个线程内要求ROS关闭

ros::this_node::getName       // 返回当前节点的名称

getAdvertisedTopics()　　//返回节点注册的话题
getSubscribedTopics()   //返回节点订阅的主题

getNamespace()	//返回节点的命名空间

ROS_ASSERT(a<b)			//同assert()，若条件不满足，程序会终止

ROS_BREAK()				//直接终止程序，报警FATAL


 // Get the hostname where the master runs. 
ROSCPP_DECL const std::string &     getHost ()

// Get the port where the master runs. 
ROSCPP_DECL uint32_t    getPort ()

// Get the full URI to the master (eg. http://host:port/) 
ROSCPP_DECL const std::string &     getURI ()

ros_Time_now不能在NodeHandle之前.png

参考：
ros 初始化和关闭