vins-fusion+ego-planner在仿真小车上的部署

OK，现在我们尝试将vins-fusion与ego-planner相结合，来实现路径规划，因为小车算是二维运动的，所以我们直接用无人机的ego-planner算法，这里我使用了https://github.com/Forrest-Z/ego-planner-for-ground-robot，这个文件实现了把ego-planner部署到小车上。

跟着教程编译完之后，我们需要做以下几步：

参数更改：

这里我们修改plan_manage/run_in_sim.launch文件中的三个参数，

<arg name="cloud_topic" value="/your_cloud_topic"/>
<arg name="odom_topic" value="/your_odom_topic"/>
<arg name="goal_topic" value="/your_odom_topic"/>

替换成你自己的话题，第二个odom_topic即为vins-fusion发布的消息

<arg name="odom_topic" value="/vins_estimator/odometry" />
<arg name="cloud_topic" value="/depth_camera/depth/points"/>
<arg name="goal_topic" value="/move_base_simple/goal"/>

哦对了，忘了说了，点云消息需要深度相机，之前跟着赵虚左老师过了一遍的话正好有一个kinect深度相机，使用的时候记得加上。

消息类型转换

改完之后，我们打开ego-planner下的traj_server.cpp文件，

ros::Publisher vel_cmd_pub;

//quadrotor_msgs::PositionCommand cmd;
geometry_msgs::TwistStamped cmd;

从前几行我们可以发现，话题发布的名称是 vel_cmd_pub，类型是TwistStamp,可是我们小车需要Twist类型的消息，这就意味着我们需要对发布的话题类型做一下转换，代码如下：

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import TwistStamped, Twist

def twist_stamped_callback(msg):
    # Extract Twist from TwistStamped message
    twist = msg.twist
    # Publish the extracted Twist message
    twist_pub.publish(twist)

if __name__ == '__main__':
    rospy.init_node('twist_stamped_to_twist_converter')
    twist_pub = rospy.Publisher('/cmd_vel/convert', Twist, queue_size=10)
    rospy.Subscriber('cmd_vel', TwistStamped, twist_stamped_callback)
    rospy.spin()

这里我们将TwistStamp类型的消息变更为了名称是“/cmd_vel/convert”，类型是Twist的消息，小车可以接受到路径规划的消息。

打开ego-planner后可以在rviz中规划，这是原文档里都有，我就不再赘述，如果你想发布自己的消息，可以创建一个发布端发布目标点的信息，如下所示：

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import PoseStamped

def send_goal():
    rospy.init_node('goal_publisher', anonymous=True)
    pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
    rospy.sleep(1)  # Wait for publisher to register

    goal = PoseStamped()
    goal.header.stamp = rospy.Time.now()
    goal.header.frame_id = "map"
    goal.pose.position.x = 1.0
    goal.pose.position.y = 2.0
    goal.pose.orientation.w = 1.0

    pub.publish(goal)
    rospy.loginfo("Goal sent!")

if __name__ == '__main__':
      try:
        send_goal()
      except rospy.ROSInterruptException:
        pass

这里我随便写了一下，大家可以根据需要修改，还有一些参数的修改可以看文档，至此，我们成功实现仿真小车中的ego-planner+vins-fusion跑通