use the planner in AirSim

[Yueting-Li]

Hi, I want to use px4_fast_planner in AirSim simulator(PX4 SITL). I modify the launch file and run. But the uav failed to avoid obstacle。It just hit the obstacle and fell. This is my active node rosgraph. Could anyone help me?

[mzahana]

Would be nice to see the interface with AirSim

[Kevinlibaba]

@Yueting-Li This error may be caused by the wrong frame of pointcloud. The frame of /points/colored is /front_center_optical_frame which should be transformed to /world.

[Yueting-Li]

@Kevinlibaba Thanks for your reply. I just use depth_image_proc (http://wiki.ros.org/depth_image_proc) to generate point cloud and don't consider the frame of it. But I dont know how to transform it. Can you give me some advice？
Hope for your reply.

[Kevinlibaba]

@Yueting-Li You just need to listen the /tf between two frames to generate a transform matrix, and make the transforming via pcl::transformPointCloud. Follows is a sample:

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <geometry_msgs/Pose.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <tf/transform_listener.h>

#include <pcl_ros/transforms.h>
#include <pcl/common/transforms.h>
#include <pcl/conversions.h>
#include <pcl_ros/impl/transforms.hpp>

class SegMapROSWraper  
{
private:
  ros::NodeHandle m_nh;  
  ros::Publisher m_globalcloudPub;  
  message_filters::Subscriber<sensor_msgs::PointCloud2> *m_pointCloudSub;  
  tf::MessageFilter<sensor_msgs::PointCloud2> *m_tfPointCloudSub;  
  tf::TransformListener m_tfListener;  

public:
  SegMapROSWraper()
      : m_nh("~")  
  {

      m_pointCloudSub = new message_filters::Subscriber<sensor_msgs::PointCloud2>(m_nh,"/seg_kitti_velo/pointcloud", 100);   
      m_tfPointCloudSub = new tf::MessageFilter<sensor_msgs::PointCloud2>(*m_pointCloudSub, m_tfListener, "/world", 100);  
      m_tfPointCloudSub->registerCallback(boost::bind(&SegMapROSWraper::insertCloudCallback, this, _1));  
      m_globalcloudPub = m_nh.advertise<sensor_msgs::PointCloud2>("/global_map", 2, true);   
  }

  ~SegMapROSWraper()
  {
      delete m_pointCloudSub;
      delete m_tfPointCloudSub;
  }

  void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud)  
  {
      pcl::PointCloud<pcl::PointXYZL> pc;
      pcl::PointCloud<pcl::PointXYZL> pc_global;
      pcl::fromROSMsg(*cloud, pc);

      tf::StampedTransform sensorToWorldTf;  
      try
      {
          m_tfListener.lookupTransform("/world", cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); 
      }
      catch (tf::TransformException &ex)
      {
          ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << ", quitting callback");
          return;
      }

      Eigen::Matrix4f sensorToWorld;
      pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);  
      pcl::transformPointCloud(pc, pc_global, sensorToWorld);   
      // std::cout<< sensorToWorld <<std::endl;
      sensor_msgs::PointCloud2 map_cloud;
      pcl::toROSMsg(pc_global, map_cloud);  
      map_cloud.header.stamp = ros::Time::now();
      map_cloud.header.frame_id = "/map"; 
      m_globalcloudPub.publish(map_cloud);  
  }
};

int main(int argc, char** argv) {

  ros::init(argc, argv, "colored"); 

  SegMapROSWraper  SM;

  ros::spin();
  return 0;
}

[Yueting-Li]

        Ok, I get it now, thanks！ ---- Replied Message ***@***.***>Date07/23/2021 18:15 ***@***.***> ***@***.******@***.***>SubjectRe: [mzahana/px4_fast_planner] use the planner in AirSim (#21)

You just need to listen the /tf between two frames to generate a transform matrix, and make the transforming via pcl::transformPointCloud. Follows is a sample: `#include <ros/ros.h>

include <pcl_ros/point_cloud.h>

include <geometry_msgs/Pose.h>

include <message_filters/subscriber.h>

include <message_filters/time_synchronizer.h>

include <tf/transform_broadcaster.h>

include <tf/message_filter.h>

include <tf/transform_listener.h>

include <pcl_ros/transforms.h>

include <pcl/common/transforms.h>

include <pcl/conversions.h>

include <pcl_ros/impl/transforms.hpp>

class SegMapROSWraper { private: ros::NodeHandle m_nh; ros::Publisher m_globalcloudPub; message_filters::Subscriber m_pointCloudSub; tf::MessageFilter m_tfPointCloudSub; tf::TransformListener m_tfListener; public: SegMapROSWraper() : m_nh("~") { m_pointCloudSub = new message_filters::Subscriber(m_nh,"/seg_kitti_velo/pointcloud", 100);
m_tfPointCloudSub = new tf::MessageFilter(*m_pointCloudSub, m_tfListener, "/world", 100);
m_tfPointCloudSub->registerCallback(boost::bind(&SegMapROSWraper::insertCloudCallback, this, _1));
m_globalcloudPub = m_nh.advertise("/global_map", 2, true);

} ~SegMapROSWraper() { delete m_pointCloudSub; delete m_tfPointCloudSub; } void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud) { pcl::PointCloudpcl::PointXYZL pc; pcl::PointCloudpcl::PointXYZL pc_global; pcl::fromROSMsg(*cloud, pc); tf::StampedTransform sensorToWorldTf;
try { m_tfListener.lookupTransform("/world", cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf);
} catch (tf::TransformException &ex) { ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << ", quitting callback"); return; }

Eigen::Matrix4f sensorToWorld; pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld);
pcl::transformPointCloud(pc, pc_global, sensorToWorld);
// std::cout<< sensorToWorld <<std::endl; sensor_msgs::PointCloud2 map_cloud; pcl::toROSMsg(pc_global, map_cloud);
map_cloud.header.stamp = ros::Time::now(); map_cloud.header.frame_id = "/map";
m_globalcloudPub .publish(map_cloud);

} }; int main(int argc, char** argv) { ros::init(argc, argv, "colored"); SegMapROSWraper SM; ros::spin(); return 0; }

—You are receiving this because you were mentioned.Reply to this email directly, view it on GitHub, or unsubscribe. [ { @.": "http://schema.org", @.": "EmailMessage", "potentialAction": { @.": "ViewAction", "target": "https://github.com/mzahana/px4_fast_planner/issues/21#issuecomment-885539876", "url": "https://github.com/mzahana/px4_fast_planner/issues/21#issuecomment-885539876", "name": "View Issue" }, "description": "View this Issue on GitHub", "publisher": { @.": "Organization", "name": "GitHub", "url": "https://github.com" } } ]

[LJJcodeing]

@Yueting-Li Hello, could you please share how you modified the file connection to AirSim? I am also trying this thing, but I have no clue. Thank you in advance for your reply