Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

split fix2pose into two separate nodes(ROS2) #316

Merged
merged 1 commit into from
Nov 29, 2023
Merged

split fix2pose into two separate nodes(ROS2) #316

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,7 +171,8 @@ To visualize the eagleye output location /eagleye/fix, for example, use the foll

To convert from eagleye/fix to eagleye/pose, use the following command　

ros2 launch eagleye_fix2pose fix2pose.xml
ros2 launch eagleye_geo_pose_fusion geo_pose_fusion.launch.xml
ros2 launch eagleye_geo_pose_converter geo_pose_converter.launch.xml

## Sample data
### ROSBAG(ROS1)
Expand Down
37 changes: 1 addition & 36 deletions eagleye_core/navigation/src/position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,42 +60,7 @@ void position_estimate_(geometry_msgs::msg::TwistStamped velocity,eagleye_msgs::
enu_pos[1] = position_status->enu_pos[1];
enu_pos[2] = position_status->enu_pos[2];

if(!position_status->gnss_update_failure)
{
gnss_status = true;

geometry_msgs::msg::PoseStamped pose;

pose.pose.position.x = enu_pos[0];
pose.pose.position.y = enu_pos[1];
pose.pose.position.z = enu_pos[2];

heading_interpolate_3rd.heading_angle = fmod(heading_interpolate_3rd.heading_angle,2*M_PI);
tf2::Transform transform;
tf2::Quaternion q;
transform.setOrigin(tf2::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z));
q.setRPY(0, 0, (90* M_PI / 180)-heading_interpolate_3rd.heading_angle);
transform.setRotation(q);

tf2::Transform transform2, transform3;
tf2::Quaternion q2(position_parameter.tf_gnss_rotation_x,position_parameter.tf_gnss_rotation_y,
position_parameter.tf_gnss_rotation_z,position_parameter.tf_gnss_rotation_w);
transform2.setOrigin(tf2::Vector3(position_parameter.tf_gnss_translation_x, position_parameter.tf_gnss_translation_y,
position_parameter.tf_gnss_translation_z));
transform2.setRotation(q2);
transform3 = transform * transform2.inverse();

tf2::Vector3 tmp_pos;
tmp_pos = transform3.getOrigin();

enu_pos[0] = tmp_pos.getX();
enu_pos[1] = tmp_pos.getY();
enu_pos[2] = tmp_pos.getZ();
}
else
{
gnss_status = false;
}
gnss_status = !position_status->gnss_update_failure;

if (heading_interpolate_3rd.status.estimate_status == true && velocity_status.status.enabled_status == true)
{
Expand Down
25 changes: 0 additions & 25 deletions eagleye_core/navigation/src/rtk_dead_reckoning.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,31 +64,6 @@ void rtk_dead_reckoning_estimate_(geometry_msgs::msg::Vector3Stamped enu_vel, nm
llh2xyz(llh_rtk,ecef_rtk);
xyz2enu(ecef_rtk,ecef_base_pos,enu_rtk);

geometry_msgs::msg::PoseStamped pose;

pose.pose.position.x = enu_rtk[0];
pose.pose.position.y = enu_rtk[1];
pose.pose.position.z = enu_rtk[2];

heading.heading_angle = fmod(heading.heading_angle,2*M_PI);
tf2::Transform transform;
tf2::Quaternion q;
transform.setOrigin(tf2::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z));
q.setRPY(0, 0, (90* M_PI / 180)-heading.heading_angle);
transform.setRotation(q);

tf2::Transform transform2;
tf2::Quaternion q2(rtk_dead_reckoning_parameter.tf_gnss_rotation_x,rtk_dead_reckoning_parameter.tf_gnss_rotation_y,rtk_dead_reckoning_parameter.tf_gnss_rotation_z,rtk_dead_reckoning_parameter.tf_gnss_rotation_w);
transform2.setOrigin(transform*tf2::Vector3(-rtk_dead_reckoning_parameter.tf_gnss_translation_x, -rtk_dead_reckoning_parameter.tf_gnss_translation_y,-rtk_dead_reckoning_parameter.tf_gnss_translation_z));
transform2.setRotation(transform*q2);

tf2::Vector3 tmp_pos;
tmp_pos = transform2.getOrigin();

enu_rtk[0] = tmp_pos.getX();
enu_rtk[1] = tmp_pos.getY();
enu_rtk[2] = tmp_pos.getZ();

if (rtk_dead_reckoning_status->position_stamp_last != gga_time && gga.gps_qual == 4)
{
rtk_dead_reckoning_status->provisional_enu_pos_x = enu_rtk[0];
Expand Down
4 changes: 3 additions & 1 deletion eagleye_rt/launch/eagleye_rt.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,9 @@
</include>
<include file="$(find-pkg-share eagleye_tf)/launch/tf.launch.xml" if="$(var use_tf)"/>

<include file="$(find-pkg-share eagleye_fix2pose)/launch/fix2pose.launch.xml">
<include file="$(find-pkg-share eagleye_geo_pose_fusion)/launch/geo_pose_fusion.launch.xml">
</include>
<include file="$(find-pkg-share eagleye_geo_pose_converter)/launch/geo_pose_converter.launch.xml">
</include>
</group>

Expand Down
Loading