22 #include "navgraph_thread.h"
24 #include <core/utils/lockptr.h>
25 #include <navgraph/constraints/constraint_repo.h>
26 #include <navgraph/yaml_navgraph.h>
28 #include <utils/math/angle.h>
42 :
Thread(
"NavGraphThread",
Thread::OPMODE_WAITFORWAKEUP),
46 #ifdef HAVE_VISUALIZATION
51 #ifdef HAVE_VISUALIZATION
54 :
Thread(
"NavGraphThread",
Thread::OPMODE_WAITFORWAKEUP),
84 cfg_abort_on_error_ =
config->
get_bool(
"/navgraph/abort_on_error");
85 #ifdef HAVE_VISUALIZATION
86 cfg_visual_interval_ =
config->
get_float(
"/navgraph/visualization_interval");
88 cfg_monitor_file_ =
false;
94 cfg_allow_multi_graph_ =
false;
96 cfg_allow_multi_graph_ =
config->
get_bool(
"/navgraph/allow_multi_graph");
100 cfg_enable_path_execution_ =
true;
102 cfg_enable_path_execution_ =
config->
get_bool(
"/navgraph/path_execution");
116 throw Exception(
"Navgraph tolerances may no longer be set in the config");
119 if (cfg_enable_path_execution_) {
125 if (!cfg_graph_file_.empty()) {
126 if (cfg_graph_file_[0] !=
'/') {
127 cfg_graph_file_ = std::string(CONFDIR) +
"/" + cfg_graph_file_;
129 graph_ = load_graph(cfg_graph_file_);
135 throw Exception(
"Graph must specify travel tolerance");
138 throw Exception(
"Graph must specify target tolerance");
141 throw Exception(
"Graph must specify orientation tolerance");
144 throw Exception(
"Graph must specify shortcut tolerance");
153 "Using target orientation time %f from graph file",
154 cfg_target_ori_time_);
158 if (cfg_log_graph_) {
162 if (cfg_monitor_file_) {
174 exec_active_ =
false;
175 target_reached_ =
false;
176 target_ori_reached_ =
false;
177 target_rotating_ =
false;
180 constrained_plan_ =
false;
186 #ifdef HAVE_VISUALIZATION
200 delete path_planned_at_;
201 delete target_reached_at_;
203 #ifdef HAVE_VISUALIZATION
204 delete visualized_at_;
210 if (cfg_enable_path_execution_) {
220 #ifdef HAVE_VISUALIZATION
222 vt_->set_constraint_repo(constraint_repo_);
223 vt_->set_graph(graph_);
232 bool needs_write =
false;
233 while (cfg_enable_path_execution_ && !pp_nav_if_->
msgq_empty()) {
243 exec_active_ =
false;
246 "Received stop message for non-active command "
247 "(got %u, running %u)",
249 pp_nav_if_->
msgid());
255 name(),
"cartesian goto (x,y,ori) = (%f,%f,%f)", msg->
x(), msg->
y(), msg->
orientation());
270 if (generate_plan(msg->
place())) {
293 if (cfg_monitor_file_) {
297 if (cfg_enable_path_execution_ && exec_active_) {
304 }
else if (target_reached_) {
310 exec_active_ =
false;
313 }
else if (target_ori_reached_) {
314 if ((now - target_reached_at_) >= target_time_) {
319 }
else if (!target_rotating_ && (now - target_reached_at_) >= target_time_) {
323 target_rotating_ =
true;
329 }
else if (target_rotating_ && node_ori_reached()) {
336 if (target_time_ == 0)
337 target_time_ = cfg_target_ori_time_;
339 target_ori_reached_ =
true;
340 target_reached_at_->
stamp();
343 }
else if (node_reached()) {
346 if (traversal_.
last()) {
351 if (target_time_ == 0)
352 target_time_ = cfg_target_time_;
354 target_reached_ =
true;
355 target_reached_at_->
stamp();
357 }
else if (traversal_.
next()) {
362 "Sending next goal %s after node reached",
371 }
else if ((shortcut_to = shortcut_possible()) > 0) {
373 "Shortcut possible, jumping from '%s' to '%s'",
375 traversal_.
path().
nodes()[shortcut_to].name().c_str());
391 bool new_plan =
false;
393 if (traversal_.
remaining() > 2 && (now - path_planned_at_) > cfg_replan_interval_) {
394 *path_planned_at_ = now;
395 constraint_repo_.
lock();
396 if (constraint_repo_->
compute() || constraint_repo_->
modified(
true)) {
405 constraint_repo_.
unlock();
408 if (!new_plan && (now - cmd_sent_at_) > cfg_resend_interval_) {
420 #ifdef HAVE_VISUALIZATION
423 if (now - visualized_at_ >= cfg_visual_interval_) {
424 *visualized_at_ = now;
425 constraint_repo_.
lock();
426 if (constraint_repo_->
compute() || constraint_repo_->
modified(
false)) {
429 constraint_repo_.
unlock();
434 if (cfg_enable_path_execution_ && needs_write) {
440 NavGraphThread::load_graph(std::string filename)
442 std::ifstream inf(filename);
443 std::string firstword;
447 if (firstword ==
"%YAML") {
457 NavGraphThread::generate_plan(
const std::string &goal_name)
461 if (cfg_enable_path_execution_) {
463 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
471 logger->
log_error(
name(),
"Failed to generate path to %s: goal is unknown", goal_name.c_str());
472 if (cfg_enable_path_execution_) {
474 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
480 return generate_plan(goal.
x(),
483 : std::numeric_limits<float>::quiet_NaN(),
490 "Starting at (%f,%f), closest node is '%s'",
491 pose_.getOrigin().x(),
492 pose_.getOrigin().y(),
493 init.name().c_str());
499 "Failed to generate path from (%.2f,%.2f) to %s: %s",
504 if (cfg_enable_path_execution_) {
506 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
511 if (!path_.
empty()) {
512 constrained_plan_ =
true;
514 constrained_plan_ =
false;
519 if (cfg_enable_path_execution_) {
521 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
530 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
539 NavGraphThread::generate_plan(
const std::string &goal_name,
float ori)
541 if (generate_plan(goal_name)) {
542 if (!path_.
empty() && std::isfinite(ori)) {
543 path_.
nodes_mutable().back().set_property(
"orientation", ori);
549 if (cfg_enable_path_execution_) {
551 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
558 NavGraphThread::generate_plan(
float x,
float y,
float ori,
const std::string &target_name)
563 "Cannot find closest node to target (%.2f,%.2f,%.2f) alias %s",
567 target_name.c_str());
570 if (generate_plan(close_to_goal.
name())) {
572 if (std::isfinite(ori)) {
573 n.set_property(
"orientation", ori);
581 if (cfg_enable_path_execution_) {
583 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
597 if (goal.
name() ==
"free-target") {
599 act_goal = close_to_goal;
607 if (!new_path.
empty()) {
609 NavGraphNode pose(
"current-pose", pose_.getOrigin().x(), pose_.getOrigin().y());
611 float new_cost = new_path.
cost();
613 if (new_cost <= old_cost * cfg_replan_factor_) {
614 constrained_plan_ =
true;
616 if (goal.
name() ==
"free-target") {
622 "Executing after re-planning from '%s' to '%s', "
623 "old cost: %f new cost: %f (%f * %f)",
624 start.name().c_str(),
627 new_cost * cfg_replan_factor_,
633 "Re-planning from '%s' to '%s' resulted in "
634 "more expensive plan: %f > %f (%f * %f), keeping old",
635 start.name().c_str(),
638 old_cost * cfg_replan_factor_,
645 "Failed to re-plan from '%s' to '%s'",
646 start.name().c_str(),
647 goal.
name().c_str());
663 NavGraphThread::optimize_plan()
668 double sqr_dist_a = (pow(pose_.getOrigin().x() - path.
nodes()[0].x(), 2)
669 + pow(pose_.getOrigin().y() - path.
nodes()[0].y(), 2));
670 double sqr_dist_b = (pow(path.
nodes()[0].x() - path.
nodes()[1].x(), 2)
671 + pow(path.
nodes()[0].y() - path.
nodes()[1].y(), 2));
672 double sqr_dist_c = (pow(pose_.getOrigin().x() - path.
nodes()[1].x(), 2)
673 + pow(pose_.getOrigin().y() - path.
nodes()[1].y(), 2));
675 if (sqr_dist_a + sqr_dist_b >= sqr_dist_c) {
682 NavGraphThread::start_plan()
684 if (!cfg_enable_path_execution_)
687 path_planned_at_->
stamp();
689 target_reached_ =
false;
690 target_ori_reached_ =
false;
691 target_rotating_ =
false;
693 exec_active_ =
false;
695 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_UNKNOWN_PLACE);
698 #ifdef HAVE_VISUALIZATION
701 visualized_at_->stamp();
708 std::string m = path_.
nodes()[0].name();
709 for (
unsigned int i = 1; i < path_.
size(); ++i) {
710 m +=
" - " + path_.
nodes()[i].name();
713 #ifdef HAVE_VISUALIZATION
715 vt_->set_traversal(traversal_);
716 visualized_at_->stamp();
742 NavGraphThread::stop_motion()
744 if (!cfg_enable_path_execution_)
755 exec_active_ =
false;
756 target_ori_reached_ =
false;
757 target_rotating_ =
false;
762 #ifdef HAVE_VISUALIZATION
765 visualized_at_->stamp();
771 NavGraphThread::send_next_goal()
773 if (!cfg_enable_path_execution_)
776 bool stop_at_target =
false;
777 bool orient_at_target =
false;
780 throw Exception(
"Cannot send next goal if plan is empty");
786 if (traversal_.
last()) {
787 stop_at_target =
true;
790 orient_at_target =
true;
802 ori = atan2f(next_next_target.
y() - next_target.
y(), next_next_target.
x() - next_target.
x());
808 tf::Vector3(next_target.
x(), next_target.
y(), 0)),
818 if (target_reached_) {
820 tpose.setOrigin(tf::Vector3(0.f, 0.f, 0.f));
825 tpose.getOrigin().y(),
826 tf::get_yaw(tpose.getRotation()));
831 if (orient_at_target) {
833 fawkes::NavigatorInterface::OrientationMode::OrientAtTarget);
836 fawkes::NavigatorInterface::OrientationMode::OrientDuringTravel);
840 #ifdef HAVE_VISUALIZATION
842 vt_->set_current_edge(last_node_, next_target.
name());
846 throw Exception(
"No writer for navigator interface");
853 "Sending goto(x=%f,y=%f,ori=%f) for node '%s'",
854 tpose.getOrigin().x(),
855 tpose.getOrigin().y(),
856 tf::get_yaw(tpose.getRotation()),
857 next_target.
name().c_str());
861 cmd_msgid_ = gotomsg->
id();
863 cmd_sent_at_->
stamp();
869 if (cfg_abort_on_error_) {
871 "Failed to send cartesian goto for "
872 "next goal, exception follows");
874 exec_active_ =
false;
876 pp_nav_if_->
set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
878 #ifdef HAVE_VISUALIZATION
888 "Failed to send cartesian goto for "
889 "next goal, exception follows");
901 NavGraphThread::node_reached()
916 float dist = sqrt(pow(pose_.getOrigin().x() - cur_target.
x(), 2)
917 + pow(pose_.getOrigin().y() - cur_target.
y(), 2));
921 if (traversal_.
last()) {
927 if (tolerance == 0.) {
932 return (dist <= tolerance);
940 NavGraphThread::node_ori_reached()
953 return node_ori_reached(cur_target);
961 NavGraphThread::node_ori_reached(
const NavGraphNode &node)
969 return (ori_diff <= ori_tolerance);
977 NavGraphThread::shortcut_possible()
979 if (!traversal_ || traversal_.
remaining() < 1) {
988 sqrt(pow(pose_.getOrigin().x() - node.
x(), 2) + pow(pose_.getOrigin().y() - node.
y(), 2));
992 if (tolerance == 0.0)
994 if (dist <= tolerance)
1014 **graph_ = **new_graph;
1021 #ifdef HAVE_VISUALIZATION
1023 vt_->set_graph(graph_);
1024 visualized_at_->stamp();
1034 bool gen_ok =
false;
1035 if (goal.
name() ==
"free-target") {
1038 gen_ok = generate_plan(goal.
name());
1052 NavGraphThread::log_graph()
1054 const std::vector<NavGraphNode> & nodes = graph_->
nodes();
1055 std::vector<NavGraphNode>::const_iterator n;
1056 for (n = nodes.begin(); n != nodes.end(); ++n) {
1058 "Node %s @ (%f,%f)%s",
1062 n->unconnected() ?
" UNCONNECTED" :
"");
1064 const std::map<std::string, std::string> & props = n->properties();
1065 std::map<std::string, std::string>::const_iterator p;
1066 for (p = props.begin(); p != props.end(); ++p) {
1071 std::vector<NavGraphEdge> edges = graph_->
edges();
1072 std::vector<NavGraphEdge>::iterator e;
1073 for (e = edges.begin(); e != edges.end(); ++e) {
1075 "Edge %10s --%s %s",
1077 e->is_directed() ?
">" :
"-",
1080 const std::map<std::string, std::string> & props = e->properties();
1081 std::map<std::string, std::string>::const_iterator p;
1082 for (p = props.begin(); p != props.end(); ++p) {
1089 NavGraphThread::publish_path()
1091 if (!cfg_enable_path_execution_)
1094 std::vector<std::string> vpath(40,
"");
1099 for (; r < traversal_.
path().size(); ++r) {
1100 vpath[ind++] = traversal_.
path().
nodes()[r].name();
virtual void fam_event(const char *filename, unsigned int mask)
Event has been raised.
virtual void once()
Execute an action exactly once.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual ~NavGraphThread()
Destructor.
virtual void loop()
Code to execute in the thread.
NavGraphThread()
Constructor.
Send Marker messages to rviz to show navgraph info.
Thread aspect provide a new aspect.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual bool exists(const char *path)=0
Check if a given value exists.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
static const unsigned int FAM_MODIFY
File was modified.
static const unsigned int FAM_IGNORED
File was ignored.
Monitors files for changes.
void watch_file(const char *filepath)
Watch a file.
void process_events(int timeout=0)
Process events.
void add_listener(FamListener *listener)
Add a listener.
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
Message * msgq_first()
Get the first message from the message queue.
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
void read()
Read from BlackBoard into local copy.
bool has_writer() const
Check if there is a writer for the interface.
LockPtr<> is a reference-counting shared lockable smartpointer.
void lock() const
Lock access to the encapsulated object.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
void unlock() const
Unlock object mutex.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
unsigned int id() const
Get message ID.
void set_navgraph(LockPtr< NavGraph > &navgraph)
Set navgraph.
bool modified(bool reset_modified=false)
Check if the constraint repo has been modified.
bool compute()
Call compute method on all registered constraints.
float x() const
Get X coordinate in global frame.
bool unconnected() const
Check if this node shall be unconnected.
bool is_valid() const
Check if node is valid, i.e.
const std::string & name() const
Get name of node.
float property_as_float(const std::string &prop) const
Get property converted to float.
bool has_property(const std::string &property) const
Check if node has specified property.
float y() const
Get Y coordinate in global frame.
bool last() const
Check if the current node is the last node in the path.
float remaining_cost() const
Get the remaining cost to the goal.
size_t current_index() const
Get index of current node in path.
void invalidate()
Invalidate this traversal.
const NavGraphNode & current() const
Get current node in path.
bool next()
Move on traversal to next node.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
size_t remaining() const
Get the number of remaining nodes in path traversal.
bool running() const
Check if traversal is currently runnung.
void set_current(size_t new_current)
Set the current node.
const NavGraphNode & peek_next() const
Peek on the next node.
Class representing a path for a NavGraph.
Traversal traversal() const
Get a new path traversal handle.
bool empty() const
Check if path is empty.
std::vector< NavGraphNode > & nodes_mutable()
Get nodes along the path as mutable vector.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
size_t size() const
Get size of path.
float cost() const
Get cost of path from start to to end.
const NavGraphNode & goal() const
Get goal of path.
void add_node(const NavGraphNode &node, float cost_from_end=0)
Add a node to the path.
void remove_change_listener(ChangeListener *listener)
Remove a change listener.
bool has_default_property(const std::string &property) const
Check if graph has specified default property.
float cost(const NavGraphNode &from, const NavGraphNode &to) const
Calculate cost between two adjacent nodes.
NavGraphNode closest_node(float pos_x, float pos_y, const std::string &property="") const
Get node closest to a specified point with a certain property.
fawkes::LockPtr< NavGraphConstraintRepo > constraint_repo() const
Get locked pointer to constraint repository.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
float default_property_as_float(const std::string &prop) const
Get property converted to float.
fawkes::NavGraphPath search_path(const std::string &from, const std::string &to, bool use_constraints=true, bool compute_constraints=true)
Search for a path between two nodes with default distance costs.
NavGraphNode node(const std::string &name) const
Get a specified node.
void apply_default_properties(NavGraphNode &node)
Set default properties on node for which no local value exists.
void add_change_listener(ChangeListener *listener)
Add a change listener.
NavPathInterface Fawkes BlackBoard Interface.
void set_path_node_1(const char *new_path_node_1)
Set path_node_1 value.
void set_path_node_17(const char *new_path_node_17)
Set path_node_17 value.
void set_path_node_40(const char *new_path_node_40)
Set path_node_40 value.
void set_path_length(const uint32_t new_path_length)
Set path_length value.
void set_path_node_15(const char *new_path_node_15)
Set path_node_15 value.
void set_path_node_6(const char *new_path_node_6)
Set path_node_6 value.
void set_path_node_13(const char *new_path_node_13)
Set path_node_13 value.
void set_path_node_24(const char *new_path_node_24)
Set path_node_24 value.
void set_path_node_23(const char *new_path_node_23)
Set path_node_23 value.
void set_path_node_8(const char *new_path_node_8)
Set path_node_8 value.
void set_path_node_18(const char *new_path_node_18)
Set path_node_18 value.
void set_path_node_36(const char *new_path_node_36)
Set path_node_36 value.
void set_path_node_26(const char *new_path_node_26)
Set path_node_26 value.
void set_path_node_25(const char *new_path_node_25)
Set path_node_25 value.
void set_path_node_29(const char *new_path_node_29)
Set path_node_29 value.
void set_path_node_7(const char *new_path_node_7)
Set path_node_7 value.
void set_path_node_3(const char *new_path_node_3)
Set path_node_3 value.
void set_path_node_35(const char *new_path_node_35)
Set path_node_35 value.
void set_path_node_34(const char *new_path_node_34)
Set path_node_34 value.
void set_path_node_32(const char *new_path_node_32)
Set path_node_32 value.
void set_path_node_27(const char *new_path_node_27)
Set path_node_27 value.
void set_path_node_39(const char *new_path_node_39)
Set path_node_39 value.
void set_path_node_38(const char *new_path_node_38)
Set path_node_38 value.
void set_path_node_28(const char *new_path_node_28)
Set path_node_28 value.
void set_path_node_4(const char *new_path_node_4)
Set path_node_4 value.
void set_path_node_21(const char *new_path_node_21)
Set path_node_21 value.
void set_path_node_14(const char *new_path_node_14)
Set path_node_14 value.
void set_path_node_11(const char *new_path_node_11)
Set path_node_11 value.
void set_path_node_2(const char *new_path_node_2)
Set path_node_2 value.
void set_path_node_37(const char *new_path_node_37)
Set path_node_37 value.
void set_path_node_5(const char *new_path_node_5)
Set path_node_5 value.
void set_path_node_33(const char *new_path_node_33)
Set path_node_33 value.
void set_path_node_30(const char *new_path_node_30)
Set path_node_30 value.
void set_path_node_31(const char *new_path_node_31)
Set path_node_31 value.
void set_path_node_20(const char *new_path_node_20)
Set path_node_20 value.
void set_path_node_12(const char *new_path_node_12)
Set path_node_12 value.
void set_path_node_16(const char *new_path_node_16)
Set path_node_16 value.
void set_path_node_9(const char *new_path_node_9)
Set path_node_9 value.
void set_path_node_22(const char *new_path_node_22)
Set path_node_22 value.
void set_path_node_10(const char *new_path_node_10)
Set path_node_10 value.
void set_path_node_19(const char *new_path_node_19)
Set path_node_19 value.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float x() const
Get x value.
float orientation() const
Get orientation value.
float y() const
Get y value.
PlaceGotoMessage Fawkes BlackBoard Interface Message.
char * place() const
Get place value.
PlaceWithOriGotoMessage Fawkes BlackBoard Interface Message.
char * place() const
Get place value.
float orientation() const
Get orientation value.
SetOrientationModeMessage Fawkes BlackBoard Interface Message.
SetStopAtTargetMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
uint32_t msgid() const
Get msgid value.
NavigatorInterface Fawkes BlackBoard Interface.
bool is_final() const
Get final value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
uint32_t msgid() const
Get msgid value.
void set_dest_y(const float new_dest_y)
Set dest_y value.
void set_dest_x(const float new_dest_x)
Set dest_x value.
void set_final(const bool new_final)
Set final value.
void unref()
Decrement reference count and conditionally delete this instance.
void ref()
Increment reference count.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
void start(bool wait=true)
Call this method to start the thread.
A class for handling time.
Time & stamp()
Set this time to the current time.
Wrapper class to add time stamp and frame ID to base types.
Fawkes library namespace.
NavGraph * load_yaml_navgraph(std::string filename, bool allow_multi_graph)
Load topological map graph stored in RCSoft format.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.