pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

added rviz config file to global fusion node

Browse Source
This commit is contained in:
admin1
2022-08-31 16:00:42 +03:00
parent 89f25ad747
commit 93c191283b
4 changed files with 300778 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
ov_msckf/src/render_cubes.cpp
View File

@@ -288,16 +288,16 @@ std::vector<std::vector<double>> cashedCubeVisPoints;
ros::Publisher pub_cubes, pub_cubes_gps, pub_aligned_traj, pub_sensor_left_gps;
ros::Subscriber sub_imu, subs_T_align, sub_stereo_left;
visualization_msgs::Marker Marker;
visualization_msgs::Marker Marker, Marker_gps;
std::ofstream fT_aligned;
nav_msgs::Path curr_path;
void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){
ROS_INFO_STREAM("received imu");
pub_cubes_gps.publish(Marker);
Marker.header.frame_id = "global";
pub_cubes.publish(Marker);
if (pub_cubes.getNumSubscribers() != 0 || pub_cubes_gps.getNumSubscribers() != 0){
pub_cubes_gps.publish(Marker_gps);
pub_cubes.publish(Marker);
}
}
void T_align_callback(const geometry_msgs::TransformStamped &msg){
@@ -434,5 +434,33 @@ int main(int argc, char* argv[]){
Marker.points.push_back(tempPoint);
}
Marker_gps.header.frame_id = "global";
Marker_gps.header.stamp = ros::Time();
Marker_gps.ns = "gps_namespace";
Marker_gps.id = 1;
Marker_gps.type = visualization_msgs::Marker::CUBE_LIST;
Marker_gps.action = visualization_msgs::Marker::ADD;
Marker_gps.pose.orientation.x = 0.0;
Marker_gps.pose.orientation.y = 0.0;
Marker_gps.pose.orientation.z = 0.0;
Marker_gps.pose.orientation.w = 1.0;
Marker_gps.scale.x = 6.8f;
Marker_gps.scale.y = 6.8f;
Marker_gps.scale.z = 6.8f;
Marker_gps.color.a = 1.0; // Don't forget to set the alpha!
Marker_gps.color.r = 0.0;
Marker_gps.color.g = 1.0;
Marker_gps.color.b = 0.0;
//only if using a MESH_RESOURCE marker type:
Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
for (auto & cubeVisPoint : cubeVisPoints){
geometry_msgs::Point tempPoint;
tempPoint.x = cubeVisPoint[0];
tempPoint.y = cubeVisPoint[1];
tempPoint.z = cubeVisPoint[2];
Marker_gps.points.push_back(tempPoint);
}
ros::spin();
}