Interface for robots based on ROS. More...
#include <vpROSRobot.h>
Public Member Functions | |
void | getDisplacement (const vpRobot::vpControlFrameType, vpColVector &) |
void | getDisplacement (const vpRobot::vpControlFrameType, vpColVector &, struct timespec ×tamp) |
void | getPosition (const vpRobot::vpControlFrameType, vpColVector &) |
void | init () |
basic initialization | |
void | init (int argc, char **argv) |
void | setCmdVelTopic (std::string topic_name) |
void | setMasterURI (std::string master_uri) |
void | setNodespace (std::string nodespace) |
void | setOdomTopic (std::string topic_name) |
void | setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &vel) |
void | stopMotion () |
vpROSRobot () | |
constructor | |
virtual | ~vpROSRobot () |
destructor | |
Protected Attributes | |
std::string | _master_uri |
std::string | _nodespace |
uint32_t | _nsec |
uint32_t | _sec |
std::string | _topic_cmd |
std::string | _topic_odom |
ros::Publisher | cmdvel |
vpColVector | displacement |
bool | isInitialized |
ros::NodeHandle * | n |
ros::Subscriber | odom |
volatile bool | odom_mutex |
vpTranslationVector | p |
vpColVector | pose_prev |
vpQuaternionVector | q |
ros::AsyncSpinner * | spinner |
Private Member Functions | |
void | get_eJe (vpMatrix &eJe) |
void | get_fJe (vpMatrix &) |
void | getArticularDisplacement (vpColVector &) |
void | getCameraDisplacement (vpColVector &) |
void | getVelocity (const vpRobot::vpControlFrameType frame, vpColVector &velocity) |
vpColVector | getVelocity (const vpRobot::vpControlFrameType frame) |
void | odomCallback (const nav_msgs::Odometry::ConstPtr &msg) |
void | setPosition (const vpRobot::vpControlFrameType, const vpColVector &) |
vpROSRobot (const vpROSRobot &robot) |
Interface for robots based on ROS.
Definition at line 62 of file vpROSRobot.h.
vpROSRobot::vpROSRobot | ( | const vpROSRobot & | robot | ) | [private] |
constructor
Definition at line 58 of file vpROSRobot.cpp.
vpROSRobot::~vpROSRobot | ( | ) | [virtual] |
destructor
Definition at line 78 of file vpROSRobot.cpp.
void vpROSRobot::get_eJe | ( | vpMatrix & | eJe | ) | [inline, private] |
Reimplemented in vpROSRobotPioneer.
Definition at line 84 of file vpROSRobot.h.
void vpROSRobot::get_fJe | ( | vpMatrix & | ) | [inline, private] |
Get the robot Jacobian expressed in the robot reference (or world) frame.
Reimplemented in vpROSRobotPioneer.
Definition at line 91 of file vpROSRobot.h.
void vpROSRobot::getArticularDisplacement | ( | vpColVector & | ) | [inline, private] |
Get a displacement expressed in the joint space between two successive position control.
Definition at line 97 of file vpROSRobot.h.
void vpROSRobot::getCameraDisplacement | ( | vpColVector & | ) | [private] |
void vpROSRobot::getDisplacement | ( | const vpRobot::vpControlFrameType | frame, |
vpColVector & | dis | ||
) |
Get the robot displacement (frame has to be specified).
frame | : Control frame. For the moment, only vpRobot::REFERENCE_FRAME is implemented. |
dis | : A 6 dimension vector that corresponds to the displacement of the robot since the last call to the function. |
vpRobotException::wrongStateError | : If the specified control frame is not supported. |
Definition at line 239 of file vpROSRobot.cpp.
void vpROSRobot::getDisplacement | ( | const vpRobot::vpControlFrameType | frame, |
vpColVector & | dis, | ||
struct timespec & | timestamp | ||
) |
Get the robot displacement (frame has to be specified).
frame | : Control frame. For the moment, only vpRobot::REFERENCE_FRAME is implemented. |
dis | : A 6 dimension vector that corresponds to the displacement of the robot since the last call to the function. |
timestamp | : timestamp of the last update of the displacement |
vpRobotException::wrongStateError | : If the specified control frame is not supported. |
Definition at line 211 of file vpROSRobot.cpp.
void vpROSRobot::getPosition | ( | const vpRobot::vpControlFrameType | frame, |
vpColVector & | pose | ||
) |
Get the robot position (frame has to be specified).
frame | : Control frame. For the moment, only vpRobot::REFERENCE_FRAME is implemented. |
pose | : A 6 dimension vector that corresponds to the position of the robot. |
vpRobotException::wrongStateError | : If the specified control frame is not supported. |
Definition at line 175 of file vpROSRobot.cpp.
void vpROSRobot::getVelocity | ( | const vpRobot::vpControlFrameType | frame, |
vpColVector & | velocity | ||
) | [private] |
Reimplemented in vpROSRobotPioneer.
vpColVector vpROSRobot::getVelocity | ( | const vpRobot::vpControlFrameType | frame | ) | [private] |
Reimplemented in vpROSRobotPioneer.
void vpROSRobot::init | ( | ) |
basic initialization
Basic initialisation
Reimplemented in vpROSRobotPioneer.
Definition at line 112 of file vpROSRobot.cpp.
void vpROSRobot::init | ( | int | argc, |
char ** | argv | ||
) |
Basic initialisation
argc,argv | : parameters of the main function |
Reimplemented in vpROSRobotPioneer.
Definition at line 93 of file vpROSRobot.cpp.
void vpROSRobot::odomCallback | ( | const nav_msgs::Odometry::ConstPtr & | msg | ) | [private] |
Definition at line 245 of file vpROSRobot.cpp.
void vpROSRobot::setCmdVelTopic | ( | std::string | topic_name | ) |
Set the ROS topic name for cmdvel
topic_name | name of the topic. |
Definition at line 285 of file vpROSRobot.cpp.
void vpROSRobot::setMasterURI | ( | std::string | master_uri | ) |
Set the URI for ROS Master
master_uri | URI of the master ("http://127.0.0.1:11311") |
Definition at line 311 of file vpROSRobot.cpp.
void vpROSRobot::setNodespace | ( | std::string | nodespace | ) |
Set the nodespace
nodespace | Namespace of the connected camera (nodespace is appended to the all topic names) |
Definition at line 323 of file vpROSRobot.cpp.
void vpROSRobot::setOdomTopic | ( | std::string | topic_name | ) |
Set the ROS topic name for odom
topic_name | name of the topic. |
Definition at line 298 of file vpROSRobot.cpp.
void vpROSRobot::setPosition | ( | const vpRobot::vpControlFrameType | , |
const vpColVector & | |||
) | [inline, private] |
Set a displacement (frame has to be specified) in position control.
Reimplemented in vpROSRobotPioneer.
Definition at line 107 of file vpROSRobot.h.
void vpROSRobot::setVelocity | ( | const vpRobot::vpControlFrameType | frame, |
const vpColVector & | vel | ||
) |
Set the velocity (frame has to be specified) that will be applied to the robot.
frame | : Control frame. For the moment, only vpRobot::REFERENCE_FRAME is implemented. |
vel | : A 6 dimension vector that corresponds to the velocities to apply to the robot. |
vpRobotException::wrongStateError | : If the specified control frame is not supported. |
Reimplemented in vpROSRobotPioneer.
Definition at line 144 of file vpROSRobot.cpp.
void vpROSRobot::stopMotion | ( | ) |
Definition at line 265 of file vpROSRobot.cpp.
std::string vpROSRobot::_master_uri [protected] |
Definition at line 78 of file vpROSRobot.h.
std::string vpROSRobot::_nodespace [protected] |
Definition at line 81 of file vpROSRobot.h.
uint32_t vpROSRobot::_nsec [protected] |
Definition at line 76 of file vpROSRobot.h.
uint32_t vpROSRobot::_sec [protected] |
Definition at line 76 of file vpROSRobot.h.
std::string vpROSRobot::_topic_cmd [protected] |
Definition at line 79 of file vpROSRobot.h.
std::string vpROSRobot::_topic_odom [protected] |
Definition at line 80 of file vpROSRobot.h.
ros::Publisher vpROSRobot::cmdvel [protected] |
Definition at line 66 of file vpROSRobot.h.
vpColVector vpROSRobot::displacement [protected] |
Definition at line 75 of file vpROSRobot.h.
bool vpROSRobot::isInitialized [protected] |
Definition at line 70 of file vpROSRobot.h.
ros::NodeHandle* vpROSRobot::n [protected] |
Definition at line 65 of file vpROSRobot.h.
ros::Subscriber vpROSRobot::odom [protected] |
Definition at line 67 of file vpROSRobot.h.
volatile bool vpROSRobot::odom_mutex [protected] |
Definition at line 77 of file vpROSRobot.h.
vpTranslationVector vpROSRobot::p [protected] |
Definition at line 73 of file vpROSRobot.h.
vpColVector vpROSRobot::pose_prev [protected] |
Definition at line 74 of file vpROSRobot.h.
vpQuaternionVector vpROSRobot::q [protected] |
Definition at line 72 of file vpROSRobot.h.
ros::AsyncSpinner* vpROSRobot::spinner [protected] |
Definition at line 68 of file vpROSRobot.h.