How to subscribe a callback for an Atlas camera
Good Evening.
The problem is my camera handlers don't work when I'm trying to run Atlas in server mode. Another handlers work.
I'm trying to subscribe to Atlas sensors: /multisense_sl/camera/left/image_raw, /multisense_sl/camera/left/camera_info, /multisense_sl/joint_states like following:
#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CameraInfo.h"
#include <sstream>
#include <iostream>
void cb_joints(const sensor_msgs::JointState::ConstPtr& msg)
{
std::cout << "joint state message: " << msg->position[0] <<
std::endl;
}
void cb_left_camera(const sensor_msgs::Image::ConstPtr& msg)
{
std::cout << "laft camera image has been received" << std::endl;
}
void cb_left_camera_info(const sensor_msgs::CameraInfo::ConstPtr& msg)
{
std::cout << "laft camera info has been received" << std::endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "get_line");
ros::NodeHandle node;
ros::Subscriber sb_joints =
node.subscribe("/multisense_sl/joint_states", 1, cb_joints);
ros::Subscriber sb_left_camera =
node.subscribe("/multisense_sl/camera/left/image_raw", 1, cb_left_camera);
ros::Subscriber sb_left_camera_info =
node.subscribe("/multisense_sl/camera/left/camera_info", 1, cb_left_camera_info);
ros::spin();
return 0;
}
So, handler 'joints' works well, but another handlers don't. I'm running Atlas without GUI (I'm working remotely) as following:
roslaunch atlas_utils atlas.launch gzname:=gzserver
and I see the following messages:
Warning [parser_urdf.cc:1158] urdf2gazebo: link[left_camera_optical_frame] has no inertia, parent joint [left_camera_optical_frame_joint] ignored
.Warning [parser_urdf.cc:1163] urdf2gazebo: link[left_camera_optical_frame] has no inertia, not modeled in gazebo
Warning [parser_urdf.cc:1158] urdf2gazebo: link[right_camera_optical_frame] has no inertia, parent joint [right_camera_optical_frame_joint] ignored
.Warning [parser_urdf.cc:1163] urdf2gazebo: link[right_camera_optical_frame] has no inertia, not modeled in gazebo
Warning [parser.cc:361] Converting a deprecated SDF source[urdf file].
Warning [Converter.cc:60] Version[1.3] to Version[1.4]
Please use the gzsdf tool to update your SDF files.
$ gzsdf convert [sdf_file]
Warning [parser.cc:286] parse from urdf file [/usr/share/drcsim-2.4/gazebo_models/atlas_description/atlas/atlas.urdf].
Error [RenderEngine.cc:608] Can't open display:
Warning [RenderEngine.cc:87] Unable to create X window. Rendering will be disabled
Error [MultiCameraSensor.cc:81] Unable to create MultiCameraSensor. Rendering is disabled.
[ WARN] [1366988210.198860577, 0.817000000]: The input topic '/multisense_sl/camera/left/image_raw' is not yet advertised
[ WARN] [1366988210.199127789, 0.818000000]: The input topic '/multisense_sl/camera/left/camera_info' is not yet advertised
[ WARN] [1366988210.199276765, 0.818000000]: The input topic '/multisense_sl/camera/right/image_raw' is not yet advertised
[ WARN] [1366988210.199400927, 0.818000000]: The input topic '/multisense_sl/camera/right/camera_info' is not yet advertised
[ WARN] [1366988270.197381765, 59.084000000]: The input topic '/multisense_sl/camera/left/image_raw' is not yet advertised
[ WARN] [1366988270.197488414, 59.084000000]: The input topic '/multisense_sl/camera/left/camera_info' is not yet advertised
[ WARN] [1366988270.197523358, 59.084000000]: The input topic '/multisense_sl/camera/right/image_raw' is not yet advertised
[ WARN] [1366988270.197571176, 59.084000000]: The input topic '/multisense_sl/camera/right/camera_info' is not yet advertised
I guess, cameras can't work in server mode (without starting GUI). Am I right? What is the simplest solution of my problem?