NODE manipulation_unit

mission manager bridge between the robocluedo ROSPlan framework and the navigation controller

This node is a bridge between the ROSPlan framework, which assumes some other node implemented the service for the navigation from rosplan, and the navigation system: a service call into a service call.

The task of this node is quite simple: when received a request from the ROSPlan module, the node associates the cartesian coordinates of the pointed waypoint, then calls the navigation service; when the navigation has finished, the node adapt and returns the response to ROSPlan.

There are also other sub-tasks performed by this node: listening for the markers in order to find the coordinates of the waypoints, for instance, using a one-shot subscriber. Or also to initialise the navigation system.

Attention: This implementation doesn’t supports the develop mode.
Authors: Francesco Ganci
Version: v1.1

Defines

NODE_NAME

LOGSQUARE(str)

OUTLABEL

TLOG(msg)

TWARN(msg)

TERR(msg)

SS(chr)

SSS(el)

SERVICE_ROBOPLAN_MANIP

SERVICE_MANIP

TOPIC_MARMERS

POSE_HOME

POSE_EXTENDED

POSE_LOW

POSE_FRONT_LOW

POSE_FRONT_HIGH

Functions

void shut_msg(int sig)

int main(int argc, char *argv[])

class node_manipulation_unit

Public Functions

inline node_manipulation_unit(): node constructor

inline void spin(): spin function: just wait for shutdown

inline bool cbk_roboplan_manip(robocluedo_rosplan_msgs::ManipulationCommand::Request &req, robocluedo_rosplan_msgs::ManipulationCommand::Response &res)

service implementation for the rosplan interface

this service behaves as a simple delegate, which takes the message from the ROSPlan side, associates a posture of the command which also depends on the quote of the selected marker, and then calls the real manipulation controller to perform the movement of the arm.

Note

the ROSplan action calls this function, in RCL#2, when the robot stands in front of a marker.

Parameters

req – the command, ROSPlan side
res – the response to ROSPlan

Returns

always true

inline void cbk_markers(const visualization_msgs::MarkerArray::ConstPtr &data)

“one-shot” listener for the markers from the Oracle

The callback is called once: after having read the markers from the topic, it shuts down the subscriber, in order to save a bit of performance.

Todo:: currently the algorithm just distinguishes between “high” markers and low markers; this approach could be improved in order to perform a better manipulation.

Parameters: data – the array of markers

Private Members

ros::NodeHandle nh: ROS node handle.

ros::ServiceServer srv_roboplan_manip: ROSPlan manipulation controller service.

ros::ServiceClient cl_manip: manipulation controller client

ros::Subscriber sub_markers: subscription handle to markers from the Oracle

bool found_markers = false: signal fro the markers listener

std::map<std::string, robocluedo_movement_controller_msgs::Pose2D> waypoints: positions of the waypoints

std::map<std::string, bool> waypoint_heigth: heigth of the markers