Commit 53f68fc1 authored by Robot5's avatar Robot5
Browse files
parents 1fd42369 6b5810a8
......@@ -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>
......@@ -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
......
......@@ -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();
}
......
......@@ -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}
......
......@@ -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);
......
......@@ -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 -->
......
......@@ -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>
......@@ -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();
......
......@@ -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
}*/
#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();
}
}
......@@ -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;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment