Last active
November 9, 2020 08:56
-
-
Save mpkuse/cc905d1ad5611177c20d541842b2493f to your computer and use it in GitHub Desktop.
Ros Quickies
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
cmake_minimum_required(VERSION 2.8.3) | |
project(roveo_sensor_calibration) | |
add_compile_options(-std=c++11) | |
find_package(catkin REQUIRED COMPONENTS | |
roscpp | |
rospy | |
std_msgs | |
) | |
# find_package(Boost REQUIRED COMPONENTS system) | |
# find_package(Ceres REQUIRED) | |
catkin_package( | |
) | |
########### | |
## Build ## | |
########### | |
include_directories( | |
# include | |
${catkin_INCLUDE_DIRS} | |
${EIGEN3_INCLUDE_DIRS} | |
) | |
#SET( SRC_FILES | |
# src/gauss_multi.cpp | |
# src/gp.cpp | |
# ) | |
# refer this as: ${SRC_FILES} | |
#add_library(vins_lib | |
# src/estimator/parameters.cpp | |
# src/estimator/estimator.cpp | |
#) | |
add_executable(lidar_imu_calib src/lidar_imu_calib.cpp) | |
target_link_libraries(lidar_imu_calib | |
#vins_lib | |
${catkin_LIBRARIES} ${PCL_LIBRARIES} | |
) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <iostream> | |
#include <string> | |
using namespace std; | |
#include <ros/ros.h> | |
#include <std_msgs/String.h> | |
#include <geometry_msgs/PoseWithCovarianceStamped.h> | |
class Fusion | |
{ | |
public: | |
Fusion() | |
{ | |
//---- Read params | |
ros::NodeHandle private_nh("~"); | |
private_nh.param("ODOMETRY_TOPIC_NAME", ODOMETRY_TOPIC_NAME, std::string("/base_pose_in_map")); | |
private_nh.param("gyro_bias", gyro_bias, -1.0f ); | |
private_nh.param("accelerometer_bias", accelerometer_bias, -1.0f ); | |
/* | |
// switch to this format of params | |
private_nh.param<double>("clustTor_search", clustTor_search, 0.1); | |
private_nh.param<bool>("useTurn", useTurn, false); | |
*/ | |
// yaml read | |
string config_file; | |
if( !private_nh.hasParam( "config_file" ) ) { | |
cout << "cannot file param `config_file`" << endl; | |
exit(1); | |
} | |
private_nh.param("config_file", config_file, std::string("no file") ); | |
auto cfg = YAMLConfigReader( config_file ); | |
//---- Subscribes and Publishers | |
cout << "Subscribe ODOMETRY_TOPIC_NAME=" << ODOMETRY_TOPIC_NAME << endl; | |
sub_lidar_odom = nh_.subscribe(ODOMETRY_TOPIC_NAME, 100, &Fusion::lidar_odom_callback, this); | |
cout << "Advertise OUTPUT_ODOM_TOPIC_NAME=" << OUTPUT_ODOM_TOPIC_NAME << endl; | |
pub_base_pose_in_map_100hz = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(OUTPUT_ODOM_TOPIC_NAME, 100); | |
} | |
//---- callbacks | |
void lidar_odom_callback( const geometry_msgs::PoseWithCovarianceStamped::ConstPtr msg ) | |
{ | |
} | |
private: | |
// ros | |
ros::NodeHandle nh_; | |
ros::Subscriber sub_lidar_odom; | |
ros::Publisher pub_base_pose_in_map_100hz; | |
// global variables | |
float gyro_bias, accelerometer_bias; | |
string ODOMETRY_TOPIC_NAME, OUTPUT_ODOM_TOPIC_NAME; | |
// helper functions | |
}; | |
int main(int argc, char ** argv ) | |
{ | |
ros::init(argc, argv, "fusion_node"); | |
std::cout<<"+++ Fusion Node +++ \n"; | |
Fusion fusion_; | |
ros::spin(); | |
return 0; | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<launch> | |
<node pkg="upsample_odom_imu_deadrecon" type="upsample_odom_ekf_v2" name="upsample_odom_ekf_v2" output="screen"> | |
<!-- TOPIC NAMES | |
--> | |
<param name="ODOMETRY_TOPIC_NAME" value="/base_pose_in_map" /> | |
<!-- sample parameters --> | |
<param name="gyro_bias" value="-1.33371e-05" /> | |
<param name="accelerometer_bias" value="-0.648316" /> | |
</node> | |
<arg name="lidar_count" default="1"/> | |
<group if="$(eval arg('lidar_count') == 2)"> | |
<node name="run_me" pkg="sample_p" type="run_me" output="screen" /> | |
</group> | |
</launch> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// Includes | |
#include <tf/transform_listener.h> | |
#include <tf/transform_datatypes.h> | |
#include <tf/transform_broadcaster.h> | |
// TF TO eIGEN. | |
// _t: time at which you want the transform | |
// wait_for_t: The amount of time to wait for transform to be available | |
// src_frame: Source frame | |
// target_frame: Target frame | |
// @returns The transform src_T_target at Eigen::Matrix4d | |
Matrix4d tf_waitForTransform( ros::Time _t, ros::Duration wait_for_, string src_frame, string target_frame ) | |
{ | |
// get from TF: imu_T_baseframe | |
cout << "tf.waitForTransform\n"; | |
tf::TransformListener tf_listener; | |
if(!tf_listener.waitForTransform(target_frame, src_frame, _t, wait_for_)) | |
{ | |
ROS_ERROR("Failed to find transform between map_frame and base_frame"); | |
exit(1); | |
} | |
tf::StampedTransform transform_base_T_imu; | |
tf_listener.lookupTransform(target_frame, src_frame, _t, transform_base_T_imu); | |
// tf --> Eigen | |
tf::Quaternion quat = transform_base_T_imu.getRotation(); | |
tf::Vector3 origin = transform_base_T_imu.getOrigin(); | |
// cout <<transform_base_T_imu << endl; | |
Matrix4d dst; | |
PoseManipUtils::raw_xyzw_to_eigenmat( | |
Vector4d(quat.x(), quat.y(), quat.z(), quat.w() ), | |
Vector3d( origin.x(), origin.y(), origin.z() ), | |
dst ); | |
return dst; | |
} | |
// Publishes the TF | |
void publish_base_pose_as_TF( ros::Time ekf__timestamp, const Matrix4d& ekf___w_T_baseframe, | |
const string frame_id, const string child_frame_id ) | |
{ | |
Vector4d quat_xyzw; Vector3d tr; | |
PoseManipUtils::eigenmat_to_raw_xyzw( ekf___w_T_baseframe, quat_xyzw, tr ); | |
static tf::TransformBroadcaster br; | |
tf::Transform transform; | |
transform.setOrigin( tf::Vector3(tr(0), tr(1), tr(2)) ); | |
transform.setRotation( tf::Quaternion( quat_xyzw(0), quat_xyzw(1), quat_xyzw(2), quat_xyzw(3) ) ); | |
br.sendTransform(tf::StampedTransform(transform, ekf__timestamp, frame_id, child_frame_id) ); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment