RobotModel_Service.cpp
Go to the documentation of this file.
00001 
00020 #include "robot_model_services/robot_model/impl/MILDRobotModelWithExactIK.hpp"
00021 #include "robot_model_services/robot_model/impl/MILDRobotModelWithApproximatedIK.hpp"
00022 #include "robot_model_services/robot_model/impl/MILDRobotModel.hpp"
00023 #include "robot_model_services/robot_model/impl/MILDRobotState.hpp"
00024 #include "robot_model_services/robot_model/RobotState.hpp"
00025 #include <ros/ros.h>
00026 #include "robot_model_services/typedef.hpp"
00027 #include "asr_robot_model_services/CalculateCameraPose.h"
00028 #include "asr_robot_model_services/CalculateCameraPoseCorrection.h"
00029 #include "asr_robot_model_services/IsPositionAllowed.h"
00030 #include "asr_robot_model_services/RobotStateMessage.h"
00031 #include "asr_robot_model_services/GetPose.h"
00032 #include "asr_robot_model_services/GetDistance.h"
00033 #include <Eigen/Dense>
00034 #include <tf/transform_datatypes.h>
00035 #include <eigen_conversions/eigen_msg.h>
00036 
00037 using namespace robot_model_services;
00038 //Since not all functionalities are implemented in the robot model with approximated IK,
00039 //two pointers are used for the basic functionalies implemented all child classes and the advanced functionalities only implmented in the RobotModelWithExactIK class
00040 MILDRobotModelPtr basicFunctionRobotModelPtr;
00041 MILDRobotModelWithExactIKPtr advancedFunctionRobotModelPtr;
00042 
00043 bool getDistance(asr_robot_model_services::GetDistance::Request &req, asr_robot_model_services::GetDistance::Response &res)
00044 {
00045       res.distance = basicFunctionRobotModelPtr->getDistance(req.sourcePosition, req.targetPosition);
00046       return true;
00047 }
00048 
00049 bool processCalculateCameraPoseServiceCall(asr_robot_model_services::CalculateCameraPose::Request &req, asr_robot_model_services::CalculateCameraPose::Response &res)
00050 {
00051     MILDRobotState * sourceRobotState = new MILDRobotState(req.sourceRobotState.pan, req.sourceRobotState.tilt,req.sourceRobotState.rotation,req.sourceRobotState.x,req.sourceRobotState.y);
00052     RobotStatePtr sourceRobotStatePtr(sourceRobotState);
00053     res.cameraFrame = basicFunctionRobotModelPtr->calculateCameraPose(sourceRobotStatePtr);
00054     return true;
00055 }
00056 
00057 bool processIsPositionAllowedServiceCall(asr_robot_model_services::IsPositionAllowed::Request &req, asr_robot_model_services::IsPositionAllowed::Response &res)
00058 {
00059   res.isAllowed = basicFunctionRobotModelPtr->isPositionAllowed(req.targetPosition);
00060   return true;
00061 }
00062 
00063 bool processGetRobotPoseServiceCall(asr_robot_model_services::GetPose::Request &req, asr_robot_model_services::GetPose::Response &res)
00064 {
00065   res.pose = basicFunctionRobotModelPtr->getRobotPose();
00066   return true;
00067 }
00068 
00069 bool processGetCameraPoseServiceCall(asr_robot_model_services::GetPose::Request &req, asr_robot_model_services::GetPose::Response &res)
00070 {
00071   res.pose = basicFunctionRobotModelPtr->getCameraPose();
00072   return true;
00073 }
00074 
00075 bool processCalculateCameraPoseCorrectionServiceCall(asr_robot_model_services::CalculateCameraPoseCorrection::Request &req, asr_robot_model_services::CalculateCameraPoseCorrection::Response &res)
00076 {
00077   MILDRobotState * sourceRobotState = new MILDRobotState(req.sourceRobotState.pan, req.sourceRobotState.tilt,req.sourceRobotState.rotation,req.sourceRobotState.x,req.sourceRobotState.y);
00078   RobotStatePtr sourceRobotStatePtr(sourceRobotState);
00079   SimpleVector3 position(req.position.x, req.position.y, req.position.z);
00080   SimpleQuaternion orientation(req.orientation.w, req.orientation.x, req.orientation.y, req.orientation.z);
00081   PTUConfig resultConfig = advancedFunctionRobotModelPtr->calculateCameraPoseCorrection(sourceRobotStatePtr, position, orientation);
00082   res.pan = std::get<0>(resultConfig);
00083   res.tilt = std::get<1>(resultConfig);
00084   return true;
00085 }
00086 
00087 int main(int argc, char *argv[])
00088 {
00089     ros::init(argc, argv, "getMovementCosts");
00090     ros::NodeHandle n = ros::NodeHandle(ros::this_node::getName());
00091 
00092     double panMin, panMax, tiltMin, tiltMax;
00093     int robotModelId;
00094     n.param("panMin", panMin, -60.);
00095     n.param("panMax", panMax, 60.);
00096     n.param("tiltMin", tiltMin, -45.);
00097     n.param("tiltMax", tiltMax, 45.);
00098     n.param("robotModelId", robotModelId, 1);
00099     ROS_INFO_STREAM("panMin: " << panMin);
00100     ROS_INFO_STREAM("panMax: " << panMax);
00101     ROS_INFO_STREAM("tiltMin: " << tiltMin);
00102     ROS_INFO_STREAM("tiltMax: " << tiltMax);
00103 
00104     advancedFunctionRobotModelPtr = MILDRobotModelWithExactIKPtr(new MILDRobotModelWithExactIK());
00105     advancedFunctionRobotModelPtr->setTiltAngleLimits(tiltMin, tiltMax);
00106     advancedFunctionRobotModelPtr->setPanAngleLimits(panMin, panMax);
00107     switch (robotModelId) {
00108     case 1:
00109         ROS_INFO_STREAM("NBV Service: Using new IK model");
00110         //Use the same robot model with exact IK for basic and advanced functionalities
00111         basicFunctionRobotModelPtr = advancedFunctionRobotModelPtr;
00112         break;
00113     case 2:
00114         ROS_INFO_STREAM("NBV Service:: Using old IK model");
00115         //Use  robot model with exact IK for advanced functionalities only, create a new robot model with approximated IK for basic functionality
00116         basicFunctionRobotModelPtr = MILDRobotModelWithApproximatedIKPtr(new MILDRobotModelWithApproximatedIK());
00117         basicFunctionRobotModelPtr->setTiltAngleLimits(tiltMin, tiltMax);
00118         basicFunctionRobotModelPtr->setPanAngleLimits(panMin, panMax);
00119         break;
00120     default:
00121         std::stringstream ss;
00122         ss << robotModelId << " is not a valid robot model ID";
00123         ROS_ERROR_STREAM(ss.str());
00124         throw std::runtime_error(ss.str());
00125     }
00126     ros::ServiceServer service_GetDistance = n.advertiseService("GetDistance", getDistance);
00127     ros::ServiceServer service_CalculateCameraPose = n.advertiseService("CalculateCameraPose", processCalculateCameraPoseServiceCall);
00128     ros::ServiceServer service_IsPositionAllowed = n.advertiseService("IsPositionAllowed", processIsPositionAllowedServiceCall);
00129     ros::ServiceServer service_GetRobotPose = n.advertiseService("GetRobotPose", processGetRobotPoseServiceCall);
00130     ros::ServiceServer service_GetCameraPose = n.advertiseService("GetCameraPose", processGetCameraPoseServiceCall);
00131     ros::ServiceServer service_CalculateCameraPoseCorrection = n.advertiseService("CalculateCameraPoseCorrection", processCalculateCameraPoseCorrectionServiceCall);
00132 
00133     ROS_INFO("RobotModel Service started.");
00134     ros::spin();
00135 
00136     return 0;
00137 }


asr_robot_model_services
Author(s): Aumann Florian, Borella Jocelyn, Heller Florian, Meißner Pascal, Schleicher Ralf, Stöckle Patrick, Stroh Daniel, Trautmann Jeremias, Walter Milena, Wittenbeck Valerij
autogenerated on Sat Jun 8 2019 18:24:52