6 #define _USE_MATH_DEFINES
35 #define DEFAULT_THREAD_PERIOD 0.01
58 yCError(LOCALIZATION2D_NWS_ROS,
"Subdevice passed to attach method is invalid");
75 yCWarning(LOCALIZATION2D_NWS_ROS) <<
"Localization data not yet available during server initialization";
79 return PeriodicThread::start();
84 if (PeriodicThread::isRunning())
86 PeriodicThread::stop();
96 yCDebug(LOCALIZATION2D_NWS_ROS) <<
"Configuration: \n" << config.
toString().c_str();
98 if (!config.
check(
"period"))
106 yCInfo(LOCALIZATION2D_NWS_ROS) <<
"Period requested: " <<
m_period;
109 if (!config.
check(
"publish_odometry"))
114 if (!config.
check(
"publish_tf"))
120 if (!config.
check(
"yarp_base_name"))
122 yCError(LOCALIZATION2D_NWS_ROS) <<
"Missing yarp_base_name parameter";
127 yCError(LOCALIZATION2D_NWS_ROS) <<
"Missing '/' in yarp_base_name parameter";
133 if (config.
check(
"subdevice"))
141 yCError(LOCALIZATION2D_NWS_ROS) <<
"Failed to open subdevice.. check params";
146 yCError(LOCALIZATION2D_NWS_ROS) <<
"Failed to open subdevice.. check params";
152 yCInfo(LOCALIZATION2D_NWS_ROS) <<
"Waiting for device to attach";
158 yCError(LOCALIZATION2D_NWS_ROS) <<
"Error initializing YARP ports";
164 yCError(LOCALIZATION2D_NWS_ROS) <<
"Error initializing ROS system";
173 if (params.
check(
"parent_frame_id"))
178 if (params.
check(
"child_frame_id"))
183 if (params.
check(
"topic_name"))
188 if (params.
check(
"node_name"))
200 yCError(LOCALIZATION2D_NWS_ROS) <<
"Opening " <<
m_node_name <<
" Node, check your yarp-ROS network configuration";
213 yCError(LOCALIZATION2D_NWS_ROS) <<
"Unable to publish data on /tf topic";
216 yCInfo(LOCALIZATION2D_NWS_ROS) <<
"ROS initialized";
235 yCTrace(LOCALIZATION2D_NWS_ROS,
"Close");
236 if (PeriodicThread::isRunning())
238 PeriodicThread::stop();
254 yCDebug(LOCALIZATION2D_NWS_ROS) <<
"Execution terminated";
262 bool ok = command.
read(connection);
272 reply.
addString(
"No commands currently available:");
276 yCError(LOCALIZATION2D_NWS_ROS) <<
"Invalid command. Try `help`";
281 if (returnToSender !=
nullptr)
283 reply.
write(*returnToSender);
294 yCInfo(LOCALIZATION2D_NWS_ROS) <<
"Running";
301 yCError(LOCALIZATION2D_NWS_ROS) <<
"getLocalizationStatus() failed";
309 yCError(LOCALIZATION2D_NWS_ROS) <<
"getCurrentPosition() failed";
327 yCWarning(LOCALIZATION2D_NWS_ROS,
"The system is not properly localized!");
331 publish_odometry_on_ROS_topic();
334 publish_odometry_on_TF_topic();
338 void Localization2D_nws_ros::publish_odometry_on_TF_topic()
347 double cosYaw = cos(halfYaw);
348 double sinYaw = sin(halfYaw);
368 void Localization2D_nws_ros::publish_odometry_on_ROS_topic()
define control board standard interfaces
constexpr yarp::conf::vocab32_t VOCAB_ERR
#define DEFAULT_THREAD_PERIOD
contains the definition of a Vector type
std::string m_robot_frame
bool m_enable_publish_odometry_tf
std::string m_fixed_frame
bool close() override
Close the DeviceDriver.
std::string m_odom_topic_name
bool initialize_YARP(yarp::os::Searchable &config)
std::string m_parent_frame_id
yarp::dev::OdometryData m_current_odometry
yarp::os::Publisher< yarp::rosmsg::nav_msgs::Odometry > m_odometry_publisher
yarp::os::Stamp m_loc_stamp
yarp::dev::Nav2D::ILocalization2D * iLoc
std::string m_rpcPortName
yarp::dev::Nav2D::Map2DLocation m_current_position
yarp::dev::PolyDriver pLoc
bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
yarp::dev::Nav2D::LocalizationStatusEnum m_current_status
yarp::os::Stamp m_odom_stamp
bool m_enable_publish_odometry_topic
yarp::os::Publisher< yarp::rosmsg::tf2_msgs::TFMessage > m_tf_publisher
bool detach() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
std::string m_child_frame_id
void run() override
Loop function.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool initialize_ROS(yarp::os::Searchable &config)
bool view(T *&x)
Get an interface to the device driver.
virtual bool getEstimatedOdometry(yarp::dev::OdometryData &odom)=0
Gets the estimated odometry the robot, including its velocity expressed in the world and in the local...
virtual bool getCurrentPosition(yarp::dev::Nav2D::Map2DLocation &loc)=0
Gets the current position of the robot w.r.t world reference frame.
virtual bool getLocalizationStatus(LocalizationStatusEnum &status)=0
Gets the current status of the localization task.
double base_vel_x
velocity of the robot [m/s] expressed in the robot reference frame
double base_vel_theta
angular velocity of the robot [deg/s] expressed in the robot reference frame
double base_vel_y
velocity of the robot [m/s] expressed in the robot reference frame
double odom_x
position of the robot [m], expressed in the world reference frame
double odom_y
position of the robot [m], expressed in the world reference frame
double odom_theta
orientation the robot [deg], expressed in the world reference frame
A container for a device driver.
bool isValid() const
Check if device is valid.
bool open(const std::string &txt)
Construct and configure a device by its common name.
void fromRotationMatrix(const yarp::sig::Matrix &R)
Converts a rotation matrix to a quaternion.
A simple collection of objects that can be described and transmitted in a portable way.
void addVocab32(yarp::conf::vocab32_t x)
Places a vocabulary item in the bottle, at the end of the list.
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
void clear()
Empties the bottle of any objects it contains.
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
void addString(const char *str)
Places a string in the bottle, at the end of the list.
An interface for reading from a network connection.
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
An interface for writing to a network connection.
An abstraction for a periodic thread.
int getOutputCount() override
Determine how many output connections this port has.
void setReader(PortReader &reader) override
Set an external reader for port data.
void interrupt() override
Interrupt any current reads or writes attached to the port.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
A class for storing options and configuration information.
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
void close() override
Stop port activity.
bool topic(const std::string &name)
Set topic to publish to.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::Publisher::write.
void write(bool forceStrict=false)
Write the current object being returned by Publisher::prepare.
Port & asPort() override
Get the concrete Port being used for communication.
A base class for nested structures that can be searched.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
double getTime() const
Get the time stamp.
int getCount() const
Get the sequence number.
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
virtual bool isString() const
Checks if value is a string.
virtual bool asBool() const
Get boolean value.
virtual std::string asString() const
Get string value.
yarp::rosmsg::geometry_msgs::Pose pose
yarp::rosmsg::geometry_msgs::Point position
yarp::rosmsg::geometry_msgs::Quaternion orientation
yarp::rosmsg::geometry_msgs::Twist twist
yarp::rosmsg::geometry_msgs::Vector3 angular
yarp::rosmsg::geometry_msgs::Vector3 linear
yarp::rosmsg::geometry_msgs::PoseWithCovariance pose
yarp::rosmsg::std_msgs::Header header
std::string child_frame_id
yarp::rosmsg::geometry_msgs::TwistWithCovariance twist
std::vector< yarp::rosmsg::geometry_msgs::TransformStamped > transforms
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
@ localization_status_localized_ok
For streams capable of holding different kinds of content, check what they actually have.
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
double now()
Return the current time in seconds, relative to an arbitrary starting point.
An interface to the operating system, including Port based communication.