Ask Your Question
0

Why is my received pose msg isn't correct?

asked 2019-02-08 22:37:57 -0500

haritsahm gravatar image

updated 2019-02-09 00:07:39 -0500

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

error_cam.png

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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.

jayess gravatar imagejayess ( 2019-02-08 22:51:02 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-02-09 03:16:29 -0500

The error here is in your ROS_INFO statement, the message transport all looks correct.

You're using %d which is the format specifier for an integer when the values in the pose message are double precision floats. If you replace %d with %f or %e then you should start seeing the correct values.

edit flag offensive delete link more

Comments

alright thanks! solved!

haritsahm gravatar imageharitsahm ( 2019-02-13 03:16:01 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-02-08 22:37:57 -0500

Seen: 36 times

Last updated: Feb 09