Merge branch 'master' of https://gitsvn-nt.oru.se/iliad/software/graph_map

graph_localization/launch/3d_mcl_ncfm2018.launch

@@ -23,6 +23,8 @@
   <param name="Velodyne" value="true" />
   <param name="PointCloud" value="false" />
   <param name="dataset" value="ncfm-2018" />
+  <param name="laser_tf" value="/velodyne" />
+  <param name="publish_sensor_link" value="true " />
 
   <!-- Topic of laser scanner -->
   <param name="points_topic" value="/$(arg robot_prefix)/sensors/velodyne_packets" />
@@ -37,7 +39,7 @@
   <param name="gt_topic" value="/$(arg robot_prefix)/kmo_navserver/state" />
 
   <!-- Choose weather to initiate pose to pose_init_<x,y,t> or the data of /<gt_topic> -->
-  <param name="initPoseFromGT" value="true" />
+  <param name="initPoseFromGT" value="false" />
   <param name="pose_init_x" value="0" />
   <param name="pose_init_y" value="0" />
   <param name="pose_init_t" value="0" />
@@ -45,23 +47,24 @@
   <!-- world frame id -->
   <param name="root_tf" value="world" />
   <!--  odometry frame id-->
-  <param name="odom_tf" value="world" />
-  <param name="base_tf" value="$(arg robot_prefix)/base_link" />
+  <param name="odom_tf" value="$(arg robot_prefix)/odom" />
+  <param name="base_tf" value="$(arg robot_prefix)/base_footprint" />
   <!--param name="mcl_tf" value="mcl_pose_est" /-->
 
 
   <!-- MCL parameters -->
 
+<param name="enable_localisation" value="true" />
   <param name="resolution_local_factor" value="1.3" />
   <param name="particle_count" value="500" />
   <param name="show_pose" value="true"/>
   <param name="fraction" value="1.0"/>
   <param name="force_SIR" value="true" />
   <param name="z_filter_height" value="1.0" />
-
-
 </node>
 
+<node name="occupancy_map_server" pkg="map_server" type="map_server" args=" $(find graph_map)/maps/occupancy_map.yaml" output="screen"/>
+
 <group if="$(arg rviz_enabled)">
   <node name="graph_map_publisher" pkg="graph_map" type="graph_map_publisher" output="screen">
     <param name="map_rate" value="1" />
@@ -77,5 +80,7 @@
   <arg name="file_1" default="2018-05-18-09-55-16.bag" />
   <node pkg="rosbag" type="play" name="player" output="screen" args="--clock -r 1.0 -q $(arg path)$(arg file_1)"/>
 </group>
-
+<node pkg="tf" type="static_transform_publisher" name="velodyne_map_static_broadcaster" args="0 0 0 0 0 0 world map_velodyne 100" />
+<node pkg="tf" type="static_transform_publisher" name="occupancy_map_static_broadcaster" args="0 0 0 0 0 0 world map 100" />
 </launch>
+

graph_localization/rviz/localization_oru_lab.rviz

@@ -12,9 +12,8 @@ Panels:
         - /Odometry2/Shape1
         - /Odometry3/Shape1
         - /Odometry4/Shape1
-        - /PointCloud22
-        - /PointCloud22/Autocompute Value Bounds1
-        - /NDT1
+        - /PointCloud21
+        - /PointCloud21/Autocompute Value Bounds1
       Splitter Ratio: 0.5
     Tree Height: 815
   - Class: rviz/Selection
@@ -63,11 +62,9 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        laser_link:
+        map:
           Value: true
-        mcl_robot_est:
-          Value: true
-        odom_base_link:
+        map_velodyne:
           Value: true
         robot4/base_footprint:
           Value: true
@@ -75,6 +72,18 @@ Visualization Manager:
           Value: true
         robot4/base_link:
           Value: true
+        robot4/base_link_ground_truth:
+          Value: true
+        robot4/camera_depth_frame:
+          Value: true
+        robot4/camera_depth_optical_frame:
+          Value: true
+        robot4/camera_link:
+          Value: true
+        robot4/camera_rgb_frame:
+          Value: true
+        robot4/camera_rgb_optical_frame:
+          Value: true
         robot4/fixed_left_wheel_link:
           Value: true
         robot4/fixed_right_wheel_link:
@@ -87,6 +96,8 @@ Visualization Manager:
           Value: true
         robot4/kinect2_rgb_optical_frame:
           Value: true
+        robot4/kinect_link:
+          Value: true
         robot4/laser2d_floor:
           Value: true
         robot4/laser2d_floor_link:
@@ -97,6 +108,8 @@ Visualization Manager:
           Value: true
         robot4/left_fork:
           Value: true
+        robot4/odom:
+          Value: true
         robot4/right_fork:
           Value: true
         robot4/robot4/velodyne:
@@ -109,6 +122,8 @@ Visualization Manager:
           Value: true
         submap_node0:
           Value: true
+        velodyne:
+          Value: true
         world:
           Value: true
       Marker Scale: 1
@@ -118,42 +133,45 @@ Visualization Manager:
       Show Names: true
       Tree:
         world:
-          mcl_robot_est:
-            laser_link:
-              {}
-          odom_base_link:
+          map:
+            {}
+          map_velodyne:
             {}
-          robot4/base_link:
+          robot4/odom:
             robot4/base_footprint:
-              {}
-            robot4/base_fork:
-              robot4/left_fork:
-                {}
-              robot4/right_fork:
-                {}
-            robot4/fixed_left_wheel_link:
-              {}
-            robot4/fixed_right_wheel_link:
-              {}
-            robot4/kinect2_link:
-              robot4/kinect2_depth_optical_frame:
-                {}
-              robot4/kinect2_rgb_optical_frame:
-                robot4/kinect2_ir_optical_frame:
+              robot4/base_link:
+                robot4/base_fork:
+                  robot4/left_fork:
+                    {}
+                  robot4/right_fork:
+                    {}
+                robot4/fixed_left_wheel_link:
                   {}
-            robot4/laser2d_floor:
-              {}
-            robot4/laser2d_floor_link:
-              {}
-            robot4/laser2d_top:
-              {}
-            robot4/laser2d_top_link:
-              {}
-            robot4/robot4/velodyne_base_link:
-              robot4/robot4/velodyne:
-                {}
-            robot4/steer_link:
-              robot4/sd_wheel_link:
+                robot4/fixed_right_wheel_link:
+                  {}
+                robot4/kinect2_link:
+                  robot4/kinect2_depth_optical_frame:
+                    {}
+                  robot4/kinect2_rgb_optical_frame:
+                    robot4/kinect2_ir_optical_frame:
+                      {}
+                robot4/kinect_link:
+                  {}
+                robot4/laser2d_floor:
+                  {}
+                robot4/laser2d_floor_link:
+                  {}
+                robot4/laser2d_top:
+                  {}
+                robot4/laser2d_top_link:
+                  {}
+                robot4/robot4/velodyne_base_link:
+                  robot4/robot4/velodyne:
+                    {}
+                robot4/steer_link:
+                  robot4/sd_wheel_link:
+                    {}
+              velodyne:
                 {}
           submap_node0:
             {}
@@ -167,36 +185,6 @@ Visualization Manager:
         {}
       Queue Size: 100
       Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 5
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.00999999978
-      Style: Flat Squares
-      Topic: /graph_node/point2_fuser
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
     - Angle Tolerance: 0.100000001
       Class: rviz/Odometry
       Covariance:
@@ -349,7 +337,7 @@ Visualization Manager:
       Topic: /ndt_mcl_pose
       Unreliable: false
       Value: true
-    - Alpha: 1
+    - Alpha: 0.800000012
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
         Max Value: 5.9265604
@@ -382,12 +370,22 @@ Visualization Manager:
     - Alpha: 0.400000006
       Class: ndt_rviz/NDT
       Color: 204; 51; 204
-      Enabled: false
+      Enabled: true
       History Length: 1
       Name: NDT
       Topic: /graph_map_publisher/NDTOmMap
       Unreliable: false
-      Value: false
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 130; 130; 130
@@ -413,25 +411,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 5.93597698
+      Distance: 18.1015873
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -12.0478163
-        Y: 10.5119715
-        Z: 0.94999975
+        X: -3.50539112
+        Y: -25.8522511
+        Z: 5.3598078e-06
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.704796195
-      Target Frame: <Fixed Frame>
+      Pitch: -0.0452038944
+      Target Frame: world
       Value: XYOrbit (rviz)
-      Yaw: 1.87858832
+      Yaw: 0.784042537
     Saved:
       - Class: rviz/Orbit
         Distance: 39.1665726

graph_localization/src/ndt_mcl_localization.cpp

@@ -62,7 +62,7 @@ class localisation_node {
   ros::Subscriber initPoseSub;
   ros::Subscriber gtPoseSub;
   ros::Subscriber PCSub;
-  tf::Transform robot_tf,sensor_tf;
+  tf::Transform Tcorr_tf, sensor_tf;
 
   Eigen::Affine3d Tsens;
   std::string rootTF;
@@ -93,6 +93,7 @@ class localisation_node {
 
   std::ofstream res;
   std::string resF_name;
+  std::string laser_link_id;
   bool initialized;
   double time_0;
 
@@ -110,6 +111,7 @@ class localisation_node {
   bool bePC;
   bool init_pose_gt;
   bool initial_pose_set;
+  bool publish_sensor_link;
   tf::TransformBroadcaster trans_pub;
   void Pose2DToTF(Eigen::Vector3d mean, ros::Time ts, Eigen::Affine3d Todometry){
     static tf::TransformBroadcaster br, br_mapOdom;
@@ -170,9 +172,10 @@ class localisation_node {
     sleep(1);
     initialized=true;
   }
-  void VeloCallback(const velodyne_msgs::VelodyneScan::ConstPtr& msg){
-    cout<<"velodyne callback"<<endl;
+  void VeloCallback(const velodyne_msgs::VelodyneScan::ConstPtr &msg){
     pcl::PointCloud<pcl::PointXYZ> cloud;
+    ros::Time t0=ros::Time::now();
+    ros::Time t_cloud_end=msg->packets[msg->packets.size()-1].stamp;
     for(size_t next = 0; next < msg->packets.size(); ++next){
       velodyne_rawdata::VPointCloud pnts;
       dataParser.unpack(msg->packets[next], pnts);
@@ -185,7 +188,8 @@ class localisation_node {
       }
       pnts.clear();
     }
-    this->processFrame(cloud,msg->header.stamp);
+    this->processFrame(cloud,t_cloud_end);
+
   }
   void PCCallback(const sensor_msgs::PointCloud2::ConstPtr& msg){
     pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
@@ -214,7 +218,7 @@ class localisation_node {
     this->processFrame(pcl_cloud,msg->header.stamp);
   }
 
-  void processFrame(pcl::PointCloud<pcl::PointXYZ> &cloud, ros::Time ts){
+  void processFrame(pcl::PointCloud<pcl::PointXYZ> &cloud, const ros::Time &ts){
     static tf::TransformListener tf_listener;
 
     pcl::PointCloud<pcl::PointXYZ> cloud_localized;
@@ -223,8 +227,8 @@ class localisation_node {
       return;
     else if(!initialized && initial_pose_set){
       geometry_msgs::Pose pose_init_geom;
-      tf::poseEigenToMsg(initial_pose,pose_init_geom);
-      Initialize(pose_init_geom,ts);
+      tf::poseEigenToMsg(initial_pose, pose_init_geom);
+      Initialize(pose_init_geom, ts);
     }
 
 
@@ -244,34 +248,39 @@ class localisation_node {
     }
 
 
-    Eigen::Affine3d T;
-    tf::poseTFToEigen(transform,T);
+    Eigen::Affine3d Todom;
+    tf::poseTFToEigen(transform, Todom);
     //Eigen::Affine3d T = getAsAffine(x, y, yaw);
     if(firstLoad){
-      tOld = T;
+      tOld = Todom;
       firstLoad = false;
     }
-    Eigen::Affine3d Tmotion = tOld.inverse() * T;
+    Eigen::Affine3d Tmotion = tOld.inverse() * Todom;
+    tOld = Todom;
 
-    tOld = T;
     transformPointCloudInPlace(Tsens,cloud);
     localisation_type_ptr_->UpdateAndPredict(cloud,Tmotion, Tsens);
     Eigen::Affine3d pose_=localisation_type_ptr_->GetPose();
 
+
+    //rootTF -> OdomTF -> baseTF    Tcorr: rootTF ->OdomTF is unknown before localization, The transformation is defined such that Trobot: rootTF -> baseTF is the robot pose in the world, Tcorr "corrects" the parent to the odoemtry frame such taht Trobot is the robot in the root
+    //                              Todom: OdomTF ->baseTF is given by the odometry estimates
+    //Trobot=Tcorr*Todom <=> Trobot*inv(Todom)
     nav_msgs::Odometry mean_pose_odom;
-    mean_pose_odom.header.frame_id="world";
+    mean_pose_odom.header.frame_id=rootTF;
     tf::poseEigenToMsg(pose_,mean_pose_odom.pose.pose);
     mean_pose_odom.header.stamp = ts;
     mclPosePub.publish(mean_pose_odom);
-    if(visualize){
-      tf::poseEigenToTF(pose_, robot_tf);
-      trans_pub.sendTransform(tf::StampedTransform( robot_tf, ts, "/world", "/mcl_robot_est") );
-      tf::poseEigenToTF(Tsens, sensor_tf);
-      trans_pub.sendTransform( tf::StampedTransform( sensor_tf, ts, "/mcl_robot_est", "/laser_link") );
-      cloud_localized.header.frame_id="/laser_link";
-      pcl_conversions::toPCL(ts,cloud_localized.header.stamp);
-      cloud_pub.publish(cloud_localized);
-    }
+
+    Eigen::Affine3d Tcorr=pose_*Todom.inverse();
+    tf::poseEigenToTF(Tcorr, Tcorr_tf);
+    trans_pub.sendTransform(tf::StampedTransform( Tcorr_tf, ts, rootTF, odomTF) );
+    tf::poseEigenToTF(Tsens, sensor_tf);
+    trans_pub.sendTransform( tf::StampedTransform( sensor_tf, ts, baseTF, laser_link_id) );
+    cloud_localized.header.frame_id=laser_link_id;
+    pcl_conversions::toPCL(ts, cloud_localized.header.stamp);
+    cloud_pub.publish(cloud_localized);
+
     cloud.clear();
     cloud_localized.clear();
   }
@@ -292,6 +301,8 @@ public:
     param.param<std::string>("odom_tf", odomTF, "/odom");
     param.param<std::string>("base_tf", baseTF, "/base_link");
     param.param<std::string>("mcl_tf", mclTF, "/mcl");
+    param.param<std::string>("laser_tf", laser_link_id, "/velodyne");
+    param.param<bool>("publish_sensor_link", publish_sensor_link, true);
 
     Eigen::Vector3d sensor_offset_pos, sensor_offset_euler;
     param.param("sensor_pose_x",sensor_offset_pos(0),0.);
@@ -376,9 +387,7 @@ public:
       initial_pose_set=true;
     }
 
-
     mclPosePub=nh.advertise<nav_msgs::Odometry>("ndt_mcl_pose", 20);
-
     cloud_pub=nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("cloud_localized", 20);
     ros::spin();
   }

graph_map/CMakeLists.txt

@@ -157,7 +157,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
     DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
     )
 
-install(TARGETS graph_map_fuser_node graph_map_publisher graph_mapping_offline show_map optimize_graph ${PROJECT_NAME}
+install(TARGETS graph_map_fuser_node graph_mapping_offline show_map optimize_graph graph_map_publisher ${PROJECT_NAME} # graph_map_server
     ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

graph_map/include/graph_map/graphfactory.h

@@ -170,7 +170,7 @@ public:
   static MapParamPtr          CreateMapParam(string MapType);
   static MapTypePtr           CreateMapType(MapParamPtr mapparam);
 
-  static GraphMapParamPtr     CreateGraphParam(const string &name);
+  //static GraphMapParamPtr     CreateGraphParam(const string &name);
   static GraphMapPtr          CreateGraph(const Eigen::Affine3d &nodepose, MapParamPtr &mapparam,GraphMapParamPtr graphparam);
   static MapNodePtr           CreateMapNode(const Eigen::Affine3d &pose,const MapParamPtr &mapparam);
   static FactorPtr            CreateObservationFactor(MapNodePtr mapPose, NodePtr observationPose,const Eigen::Affine3d &diff,const Matrix6d &covar);

graph_map/launch/3d_fuser_ncfm2018.launch

@@ -14,6 +14,7 @@
 </group>
 
 
+
 <!-- start mapping node -->
 <node pkg="graph_map" type="graph_map_fuser_node" name="graph_node" output="screen">
 
@@ -82,7 +83,7 @@
       <!--<param name="laser_variance_z" value="0.02" /> -->
 
       <!--range of sensor, min range can prevent self-mapping of the robot or operator of the lidar, max range can remove uncertain measurments -->
-      <param name="max_range" value="100." />
+      <param name="max_range" value="130." />
       <param name="min_range" value="1.8" />
 
       <!-- Specific sensor offset parameters with respect to the odometry frame -->

graph_map/launch/open_map/graph_map_server.launch

@@ -3,11 +3,13 @@
  <!-- ros_args.launch -->
 <arg name="file_name" default="$(find graph_map)/maps/ndt_map.MAP"/>
 <arg name="map_topic" default="/maps/map_velodyne"/>
-<node pkg="graph_map" type="graph_map_server" name="graph_map_server" args="" output="screen">
+<!--node pkg="graph_map" type="graph_map_server" name="graph_map_server" args="" output="screen">
   <param name="map_topic" value="$(arg map_topic)"/>
   <param name="file_name" value="$(arg file_name)"/>
-</node>
-<node pkg="rviz" type="rviz" name="rviz" args="-d $(find graph_map)/rviz/showMap.rviz" />
+</node-->
+<!--node pkg="rviz" type="rviz" name="rviz" args="-d $(find graph_map)/rviz/showMap.rviz" /-->
+<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 world map_velodyne 100" />
+
 </launch>
 
 

graph_map/src/graph_map/map_type.cpp

@@ -42,6 +42,7 @@ void MapType::updateMap(const Eigen::Affine3d &Tnow,pcl::PointCloud<pcl::PointXY
     observations_++;
     if(store_points_)
       input_cloud_.push_back(cloud);
+
     //const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> ptr=boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(&cloud);
     //octree.setInputCloud(ptr);
     //octree.addPointsFromInputCloud();

graph_map/src/graph_map_conversions.cpp

@@ -20,7 +20,9 @@ void ToMessage(graph_map::GraphMapMsg &msg, const pogm::GraphMapNavigatorPtr &gr
   }
 }
 
-
-/*void FromMessage(const graph_map::GraphMapMsg &msg, pogm::GraphMapNavigatorPtr &graph_map){
-  graph_map=pogm::GraphFactory::CreateGraphParam(msg.map_type)
+/*
+void FromMessage(const graph_map::GraphMapMsg &msg, pogm::GraphMapNavigatorPtr &graph_map){
+  pogm::GraphMapNavigatorParamPtr graph_map=new pogm::GraphMapNavigatorParam();
+  graph_map graph_map
 }*/
+

graph_map/src/graph_map_server.cpp

+
+#include "graph_map/graph_map_fuser.h"
+#include "graph_map/factor.h"
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <std_srvs/Empty.h>
+
+#include <ndt_map/NDTMapMsg.h>
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+
+#include <ros/ros.h>
+#include <iostream>
+#include "graph_map/GraphMapMsg.h"
+#include "graph_map/GetGraphMap.h"
+#include "graph_map/graphfactory.h"
+#include "graph_map/graph_map_navigator.h"
+#include <boost/program_options.hpp>
+#include "graph_map/graph_map_conversions.h"
+
+namespace pogm = perception_oru::graph_map;
+namespace gm = graph_map;
+using graph_map::GraphMapMsg;
+GraphMapMsg msg;
+
+bool callback(gm::GetGraphMap::Request &req,
+              gm::GetGraphMap::Response &res) {
+  res.graphmap = msg;
+  return true;
+}
+
+int main(int argn, char *argv[]) {
+
+  ros::init(argn, argv, "graph_map_server");
+  std::string file_name, map_topic;
+
+  pogm::GraphMapNavigatorPtr graph_map_ptr;
+  /* if (argn == 2) {
+    std::cout << "Usage cliffmap_server <file_name>" << std::endl;
+    return -1;
+  }*/
+
+  ros::NodeHandle nh("~");
+
+  nh.param<std::string>("map_topic", map_topic, "/maps/map_velodyne");
+  nh.param<std::string>("file_name", file_name, "ndt_map.map");
+  pogm::LoadGraphMap(file_name,graph_map_ptr);
+  ros::Publisher graph_map_pub = nh.advertise<GraphMapMsg>(map_topic, 10);
+  ROS_INFO("Graph map will be published when there is a subscriber.");
+  ros::ServiceServer service_ = nh.advertiseService("get_graphmap", callback);
+  ToMessage(msg, graph_map_ptr);
+  while (ros::ok()) {
+    if (graph_map_pub.getNumSubscribers() > 0)
+      graph_map_pub.publish(msg);
+    ros::spinOnce();
+  }
+
+}

graph_map/src/graphfactory.cpp

@@ -109,11 +109,8 @@ GraphMapPtr GraphFactory::CreateGraph(const Eigen::Affine3d &nodepose, MapParamP
     GraphMapPtr graphPtr=GraphMapPtr(new GraphMapNavigator(nodepose,mapparam,graphparam));
     return graphPtr;
   }
-  else{
-    if(!disable_output_)
-      cout<<"Graphfactory: parameter NULL to graph"<<endl;
-    return NULL;
-  }
+  else
+      cerr<<"Graphfactory: mapp parameters are NULL, no graph created"<<endl;
 }