diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 604760d..8830579 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index 6811104..72f8548 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/src/APIs/c_api/include/nav_c_api.h @@ -35,10 +35,12 @@ extern "C" void *ptr; } NavigationHandle; - typedef struct - { - void *ptr; - } TFListenerHandle; + // typedef struct + // { + // void *ptr; + // } TFListenerHandle; + + typedef void* TFListenerHandle; typedef enum { diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 0bd1393..9611e7b 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -85,9 +85,9 @@ extern "C" TFListenerHandle tf_listener_create(void) extern "C" void tf_listener_destroy(TFListenerHandle handle) { - if (handle.ptr) + if (handle) { - auto *tf_ptr = static_cast *>(handle.ptr); + auto *tf_ptr = static_cast *>(handle); delete tf_ptr; } } @@ -98,14 +98,14 @@ extern "C" bool tf_listener_set_static_transform(TFListenerHandle tf_handle, double x, double y, double z, double qx, double qy, double qz, double qw) { - if (!tf_handle.ptr || !parent_frame || !child_frame) + if (!tf_handle || !parent_frame || !child_frame) { return false; } try { - auto *tf_ptr = static_cast *>(tf_handle.ptr); + auto *tf_ptr = static_cast *>(tf_handle); if (!tf_ptr || !(*tf_ptr)) { return false; @@ -176,7 +176,7 @@ extern "C" void navigation_destroy(NavigationHandle handle) extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) { printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__); - if (!handle.ptr || !tf_handle.ptr) + if (!handle.ptr || !tf_handle) { printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__); return false; @@ -188,7 +188,7 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle if (!nav_ptr || !*nav_ptr) return false; auto *nav = nav_ptr->get(); - auto *tf_ptr = static_cast *>(tf_handle.ptr); + auto *tf_ptr = static_cast *>(tf_handle); if (!tf_ptr || !(*tf_ptr)) { printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__); diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index 44b6cdf..029309f 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -85,6 +85,7 @@ void mkt_algorithm::diff::GoStraight::initialize( mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { + // robot::log_info("x %f y %f theta %f", pose.pose.x, pose.pose.y, pose.pose.theta); mkt_msgs::Trajectory2D result; result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; if (!traj_) diff --git a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp index 9b7b5bd..3126c0e 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp @@ -49,7 +49,11 @@ namespace robot_nav_core_adapter { - LocalPlannerAdapter::LocalPlannerAdapter() : has_active_goal_(false) + LocalPlannerAdapter::LocalPlannerAdapter() + : initialized_(false), + tf_(nullptr), + costmap_robot_(nullptr), + has_active_goal_(false) { } @@ -139,6 +143,15 @@ namespace robot_nav_core_adapter { return false; } + if(tf_ == nullptr) + { + robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__); + throw std::runtime_error("Failed to create the tf"); + } + else + { + robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__); + } if (planner_name != last_config_.planner_name) { robot_nav_core2::LocalPlanner::Ptr old_planner = planner_; @@ -157,15 +170,6 @@ namespace robot_nav_core_adapter robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str()); throw std::runtime_error("Failed to load planner " + planner_name); } - if(tf_ == nullptr) - { - robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__); - throw std::runtime_error("Failed to create the tf"); - } - else - { - robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__); - } new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_); planner_ = new_planner; planner_loader_ = new_loader; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 3fb3859..bff0768 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -341,6 +341,8 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) } initialized_ = true; setup_ = true; + if(tf_) + robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); robot::log_info("========== End: initialize() - SUCCESS =========="); } else @@ -715,13 +717,38 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na // robot::log_info("addOdometry: %s", odometry_name.c_str()); // robot::log_info("odometry header: %s", odometry.header.frame_id.c_str()); // robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str()); - // robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x); - // robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y); - // robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z); - // robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x); - // robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y); - // robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z); - // robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w); + if(tf_) + { + // try + // { + // robot_geometry_msgs::PoseStamped global_pose_stamped; + // tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose); + // robot_geometry_msgs::PoseStamped robot_pose; + // tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); + // robot_pose.header.frame_id = robot_base_frame_; + // robot_pose.header.stamp = robot::Time(); + // robot::Time current_time = robot::Time::now(); // save time for checking tf delay later + + // // Convert robot::Time to tf3::Time + // tf3::Time tf3_current_time = data_convert::convertTime(current_time); + // tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time()); + + // std::string error_msg; + // if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg)) + // { + // // Transform is available at current time + // tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time); + // tf3::doTransform(robot_pose, global_pose_stamped, transform); + // robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation)); + // } + // } + // catch (const tf3::TransformException &ex) + // { + // robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what()); + // } + robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); + } + // robot::log_info("odometry x: %f y: %f theta: %f", odometry.pose.pose.position.x, odometry.pose.pose.position.y, data_convert::getYaw(odometry.pose.pose.orientation)); // std::stringstream pose_covariance_str; // for(int i = 0; i < 36; i++) { // pose_covariance_str << odometry.pose.covariance[i] << " ";