cl_nav2z Namespace Reference

Namespaces
namespace	backward_global_planner
namespace	backward_local_planner
namespace	forward_global_planner
namespace	forward_local_planner
namespace	odom_tracker
namespace	pure_spinning_local_planner
namespace	undo_path_global_planner

Classes
class	CbAbortNavigation
class	CbAbsoluteRotate
class	CbNav2ZClientBehaviorBase
class	CbNavigateBackwards
class	CbNavigateForward
struct	CbNavigateForwardOptions
class	CbNavigateGlobalPosition
struct	CbNavigateGlobalPositionOptions
class	CbNavigateNamedWaypoint
class	CbNavigateNextWaypoint
class	CbNavigateNextWaypointUntilReached
class	CbPauseSlam
class	CbResumeSlam
class	CbRetry
class	CbRotate
class	CbRotateLookAt
class	CbSeekWaypoint
class	CbStopNavigation
class	CbUndoPathBackwards
struct	CbUndoPathBackwardsOptions
class	CbWaitNav2Nodes
class	CbWaitPose
class	CbWaitTransform
class	ClNav2Z
class	CpAmcl
class	CpCostmapProxy
class	CpCostmapSwitch
class	CpGoalCheckerSwitcher
class	CpPlannerSwitcher
class	CpSlamToolbox
class	CpWaypointNavigator
class	CpWaypointNavigatorBase
class	CpWaypointsVisualizer
struct	EvGoalWaypointReached
struct	EvWaypointFinal
struct	NavigateNextWaypointOptions
class	Pose
struct	Pose2D

Typedefs
typedef smacc2::client_bases::SmaccActionClientBase< nav2_msgs::action::NavigateToPose >	Base
typedef Base::WrappedResult	WrappedResult

Enumerations
enum class	SpinningPlanner { Default , PureSpinning , Forward }
enum class	Nav2Nodes {   PlannerServer , ControllerServer , RecoveriesServer , BtNavigator ,   MapServer , None }
enum class	WaitPoseStandardReferenceFrame { Map , Odometry }
enum class	StandardReferenceFrames { Map , Odometry }

Functions
geometry_msgs::msg::PoseStamped	makePureSpinningSubPlan (const geometry_msgs::msg::PoseStamped &start, double dstRads, std::vector< geometry_msgs::msg::PoseStamped > &plan, double radstep=0.005)
geometry_msgs::msg::PoseStamped	makePureStraightSubPlan (const geometry_msgs::msg::PoseStamped &startOrientedPose, const geometry_msgs::msg::Point &goal, double length, std::vector< geometry_msgs::msg::PoseStamped > &plan)
std::string	toString (Nav2Nodes value)
Nav2Nodes	fromString (std::string str)
std::string	referenceFrameToString (StandardReferenceFrames referenceFrame)

Variables
const std::string	frameid = "map"

Typedef Documentation

Base

typedef smacc2::client_bases::SmaccActionClientBase<nav2_msgs::action::NavigateToPose> cl_nav2z::Base

Definition at line 28 of file nav2z_client.cpp.

WrappedResult

typedef Base::WrappedResult cl_nav2z::WrappedResult

Definition at line 29 of file nav2z_client.cpp.

Enumeration Type Documentation

Nav2Nodes

enum class cl_nav2z::Nav2Nodes

strong

Enumerator
PlannerServer
ControllerServer
RecoveriesServer
BtNavigator
MapServer
None

Definition at line 29 of file cb_wait_nav2_nodes.hpp.

{
  PlannerServer,
  ControllerServer,
  RecoveriesServer,
  BtNavigator,
  MapServer,
  None
};

SpinningPlanner

enum class cl_nav2z::SpinningPlanner

strong

Enumerator
Default
PureSpinning
Forward

Definition at line 61 of file cb_nav2z_client_behavior_base.hpp.

{
  Default,
  PureSpinning,
  Forward
};

StandardReferenceFrames

enum class cl_nav2z::StandardReferenceFrames

strong

Enumerator
Map
Odometry

Definition at line 37 of file cp_pose.hpp.

{
  Map,
  Odometry
};

WaitPoseStandardReferenceFrame

enum class cl_nav2z::WaitPoseStandardReferenceFrame

strong

Enumerator
Map
Odometry

Definition at line 28 of file cb_wait_pose.hpp.

{
  Map,
  Odometry
};

Function Documentation

fromString()

Nav2Nodes cl_nav2z::fromString

(

std::string

str

)

Definition at line 117 of file cb_wait_nav2_nodes.cpp.

{
  if (id == "planner_server")
    return Nav2Nodes::PlannerServer;
  else if (id == "controller_server")
    return Nav2Nodes::ControllerServer;
  else if (id == "behavior_server")
    return Nav2Nodes::RecoveriesServer;
  else if (id == "bt_navigator")
    return Nav2Nodes::BtNavigator;
  else if (id == "map_server")
    return Nav2Nodes::MapServer;
  else
    return Nav2Nodes::None;
}

References BtNavigator, ControllerServer, MapServer, None, PlannerServer, and RecoveriesServer.

Referenced by cl_nav2z::CbWaitNav2Nodes::onMessageReceived().

Here is the caller graph for this function:

makePureSpinningSubPlan()

geometry_msgs::msg::PoseStamped cl_nav2z::makePureSpinningSubPlan	(	const geometry_msgs::msg::PoseStamped &	start,
		double	dstRads,
		std::vector< geometry_msgs::msg::PoseStamped > &	plan,
		double	radstep = 0.005
	)

Definition at line 37 of file common.cpp.

{
  double startYaw = tf2::getYaw(start.pose.orientation);
  // RCLCPP_INFO(getLogger(),"pure spining start yaw: %lf", startYaw);
  // RCLCPP_INFO(getLogger(),"pure spining goal yaw: %lf", dstRads);
  // RCLCPP_WARN_STREAM(getLogger(),"pure spinning start pose: " << start);
 
  double goalAngleOffset = angles::shortest_angular_distance(startYaw, dstRads);
  // RCLCPP_INFO(getLogger(),"shortest angle: %lf", goalAngleOffset);
 
  if (goalAngleOffset >= 0)
  {
    // angle positive turn counterclockwise
    // RCLCPP_INFO(getLogger(),"pure spining counterclockwise");
    for (double dangle = 0; dangle <= goalAngleOffset; dangle += radstep)
    {
      geometry_msgs::msg::PoseStamped p = start;
      double yaw = startYaw + dangle;
      // RCLCPP_INFO(getLogger(),"pure spining counterclockwise, current path yaw: %lf, dangle: %lf,
      // angleoffset %lf, radstep %lf pathsize(%ld)", yaw, dangle, goalAngleOffset, radstep, plan.size());
      tf2::Quaternion q;
      q.setRPY(0, 0, yaw);
      p.pose.orientation = tf2::toMsg(q);
      plan.push_back(p);
    }
  }
  else
  {
    // angle positive turn clockwise
    // RCLCPP_INFO(getLogger(),"pure spining clockwise");
    for (double dangle = 0; dangle >= goalAngleOffset; dangle -= radstep)
    {
      // RCLCPP_INFO(getLogger(),"dangle: %lf", dangle);
      geometry_msgs::msg::PoseStamped p = start;
      double yaw = startYaw + dangle;
      // RCLCPP_INFO(getLogger(),"pure spining clockwise, yaw: %lf, dangle: %lf, angleoffset %lf radstep
      // %lf", yaw, dangle, goalAngleOffset,radstep);
      tf2::Quaternion q;
      q.setRPY(0, 0, yaw);
      p.pose.orientation = tf2::toMsg(q);
      plan.push_back(p);
    }
  }
 
  // RCLCPP_INFO(getLogger(),"pure spining end yaw: %lf", dstRads);
  geometry_msgs::msg::PoseStamped end = start;
  tf2::Quaternion q;
  q.setRPY(0, 0, dstRads);
  end.pose.orientation = tf2::toMsg(q);
  plan.push_back(end);
 
  return end;
}

Referenced by cl_nav2z::backward_global_planner::BackwardGlobalPlanner::createDefaultBackwardPath(), and cl_nav2z::forward_global_planner::ForwardGlobalPlanner::createPlan().

Here is the caller graph for this function:

makePureStraightSubPlan()

geometry_msgs::msg::PoseStamped cl_nav2z::makePureStraightSubPlan	(	const geometry_msgs::msg::PoseStamped &	startOrientedPose,
		const geometry_msgs::msg::Point &	goal,
		double	length,
		std::vector< geometry_msgs::msg::PoseStamped > &	plan
	)

Definition at line 93 of file common.cpp.

{
  double dx = 0.01;  // 1 cm
  double steps = length / dx;
  double dt = 1.0 / steps;
 
  // geometry_msgs::msg::PoseStamped end;
  // end.pose.orientation = startOrientedPose.pose.orientation;
  // end.pose.position = goal;
  plan.push_back(startOrientedPose);
 
  // RCLCPP_INFO_STREAM(nh_->get_logger(),"Pure straight, start: " << startOrientedPose << std::endl << "end: " <<
  // goal);
  for (double t = 0; t <= 1.0; t += dt)
  {
    geometry_msgs::msg::PoseStamped p = startOrientedPose;
 
    p.pose.position.x = startOrientedPose.pose.position.x * (1 - t) + goal.x * t;
    p.pose.position.y = startOrientedPose.pose.position.y * (1 - t) + goal.y * t;
    p.pose.orientation = startOrientedPose.pose.orientation;
 
    plan.push_back(p);
  }
 
  return plan.back();
}

Referenced by cl_nav2z::backward_global_planner::BackwardGlobalPlanner::createDefaultBackwardPath(), and cl_nav2z::forward_global_planner::ForwardGlobalPlanner::createPlan().

Here is the caller graph for this function:

referenceFrameToString()

std::string cl_nav2z::referenceFrameToString

(

StandardReferenceFrames

referenceFrame

)

Definition at line 42 of file cp_pose.cpp.

{
  switch (referenceFrame)
  {
    case StandardReferenceFrames::Map:
      return "map";
    case StandardReferenceFrames::Odometry:
      return "odom";
    default:
      return "odom";
  }
}

References Map, and Odometry.

toString()

std::string cl_nav2z::toString

(

Nav2Nodes

value

)

Definition at line 98 of file cb_wait_nav2_nodes.cpp.

{
  switch (value)
  {
    case Nav2Nodes::PlannerServer:
      return "planner_server";
    case Nav2Nodes::ControllerServer:
      return "controller_server";
    case Nav2Nodes::RecoveriesServer:
      return "behavior_server";
    case Nav2Nodes::BtNavigator:
      return "bt_navigator";
    case Nav2Nodes::MapServer:
      return "map_server";
    default:
      return "";
  }
}

References BtNavigator, ControllerServer, MapServer, PlannerServer, and RecoveriesServer.

Referenced by cl_nav2z::CbWaitNav2Nodes::onEntry(), and cl_nav2z::CbWaitNav2Nodes::onMessageReceived().

Here is the caller graph for this function:

Variable Documentation

frameid

const std::string cl_nav2z::frameid = "map"

Definition at line 25 of file cp_waypoints_visualizer.cpp.

Referenced by cl_nav2z::CpWaypointsVisualizer::createMarker(), and cl_nav2z::CpWaypointsVisualizer::createMarkerLabel().