service has not been advertised
Hi,
I created the a service and I implemented the service client but the service is not being advertised I could not know what is the source of the problem. I tried the following :
file 1 name : positionEstimationServer.cpp
#include "ros/ros.h"
#include <ros/package.h>
#include <tf/tf.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include "pkg2_msgs/PoseEsti.h"
#include "pkg2_msgs/PES.h" // This is the service
#include <pkg1_msgs/Object.h>
class PositionEstimationServer
{
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, nav_msgs::Odometry> MySyncPolicy;
protected:
pkg2_msgs::PoseEsti goalCam;
pkg1_msgs::Object goalW;
message_filters::Subscriber<sensor_msgs::Image> *img_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> *camera_info_sub_;
message_filters::Subscriber<nav_msgs::Odometry> *uav_odom_sub_;
message_filters::Synchronizer<MySyncPolicy> *sync;
bool flag_done ;
public:
ros::NodeHandle nh_;
PositionEstimationServer(std::string name)
{
std::cout << "Constructor 1 " << std::endl << std::flush ;
this->flag_done = false;
img_sub_ = new message_filters::Subscriber<sensor_msgs::Image>(nh_, "/downward_cam/camera/image", 10);
camera_info_sub_ = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nh_, "/downward_cam/camera/camera_info", 10);
uav_odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh_, "/mavros/local_position/odom", 10);
std::cout << "declared Subscriber " << std::endl << std::flush ;
ros::ServiceServer service = nh_.advertiseService("poseEstimationServer",&PositionEstimationServer::poseEstimateFunction, this);
std::cout << "started the service" << std::endl << std::flush ;
sync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(1000), *img_sub_, *camera_info_sub_, *uav_odom_sub_);
sync->registerCallback(boost::bind(&PositionEstimationServer::imageCallback, this, _1, _2, _3));
BaseToCamera.setOrigin(tf::Vector3(0.0, 0.0, -0.045l));
BaseToCamera.setRotation(tf::Quaternion(0.707, -0.707, 0.000, -0.000));
}
~PositionEstimationServer(void)
{
}
bool poseEstimateFunction(pkg2_msgs::PES::Request &req,
pkg2_msgs::PES::Response &res)
{
std::cout << "waiting for goal" << std::endl << std::flush ;
goalCam.x = req.uv.x ;
goalCam.y = req.uv.y ;
while (ros::ok()) {
ros::spinOnce();
loopRate.sleep();
if (this->flag_done)
{
std::cout << "Done" << std::endl << std::flush ;
res.obj = goalW;
return true ;
}
else
{
//std::cout << "Continue" << std::endl << std::flush ;
return false;
continue ;
}
}
}
//call back, for processing
void imageCallback(const sensor_msgs::ImageConstPtr& img,
const sensor_msgs::CameraInfoConstPtr& cam_info,
const nav_msgs::OdometryConstPtr& odom
)
{
std::cout << "Image Call Back" << std::endl << std::flush ;
//Processing and filling the goalW
flag_done = true ;
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "poseEstimationServer");
PositionEstimationServer poseEstimatServerObject(ros::this_node::getName());
ros::spin();
return 0;
}
The Service that has been created as the following:
pkg2_msgs/PoseEsti uv
---
pkg1_msgs/Object obj
The client file name: positionEstimationClient.cpp as follows:
#include "ros/ros.h"
#include "pkg2_msgs/PES.h"
#include "pkg2_msgs/PoseEsti.h"
#include "pkg1_msgs/Object.h"
#include <cstdlib>
int main(int argc, char **argv)
{
ros::init(argc, argv, "poseEstimationClient");
ros::NodeHandle n;
ros::service::waitForService("poseEstimationServer");
ros::ServiceClient client = n.serviceClient<pkg2_msgs::PES>("poseEstimationServer");
pkg2_msgs::PES srv;
srv.request.uv.x = 100;
srv.request.uv.y = 100;
pkg1_msgs::Object w ;
std::cout << "calling the servise " << client.call(srv) << std::endl << std::flush ;
while (ros::ok())
{
if (client.call(srv))
{
w = srv.response.obj ;
//ROS_INFO(srv.response.obj.pose.pose.position.x);
}
else
{
ROS_ERROR("Failed to call service position Estimation");
//return 1 ...
What is the output of
rosservice list
?