Why is my received pose msg isn't correct?
Hi, I was trying to publish a camera pose using geometry_pose. But the value received and the expected value/checked via rostopic echo is different? Am I doing something wrong?
Here's my code snippet
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <vision_simple/vision_core.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "vision_simple_node");
VisionCore vision_core_;
ros::NodeHandle nh_;
ros::Subscriber base_pose = nh_.subscribe("/robot_state/camera_pose", 10, &VisionCore::CameraPoseCallback, &vision_core_);
ros::Rate loop_rate(30);
while(ros::ok())
{
if(vision_core_.newImage())
{
vision_core_.process();
vision_core_.publish();
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
this is the function
void VisionCore::CameraPoseCallback(const geometry_msgs::PoseConstPtr &msg)
{
geometry_msgs::Pose cam_pose_;
cam_pose_.position = msg->position;
cam_pose_.orientation = msg->orientation;
ROS_INFO("Camera State z : %d", msg->position.z);
tf::poseMsgToEigen(cam_pose_, camera);
}
here is the output
This is my cmake and xml
cmake_minimum_required(VERSION 2.8.3)
project(vision_simple)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp geometry_msgs
std_msgs message_generation
cv_bridge image_transport
sensor_msgs robotis_math
gazebo_plugins eigen_conversions
)
set(OpenCV_DIR "usr/local/share/OpenCV/")
set(yaml-cpp_DIR "usr/local/lib/cmake/yaml-cpp/")
find_package(OpenCV 3.4 REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(Eigen3 REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
## Generate messages in the 'msg' folder
add_message_files(
FILES
Points.msg
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs geometry_msgs
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS
roscpp
geometry_msgs
std_msgs
message_generation
cv_bridge
image_transport
DEPENDS OpenCV
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
${YAML_CPP_INCLUDE_DIR}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/vision_simple_node.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node
src/vision_simple_node.cpp
src/vision_core.cpp
src/linedetection.cpp
src/fielddetection.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX ...
Please don't use images to display text. Images are not searchable and people cannot copy and paste the text from the image. Please update your question with a copy and paste of the text instead of using images.