Need help with code

asked 2022-05-31 21:19:10 -0500

lutinplunder gravatar image

updated 2022-06-01 06:12:22 -0500

Joe28965 gravatar image

I am trying to figure out how to write the code needed to take the angular Y value from my topic /shc/pose then possibly modify it to be negative (if needed for the servo to move in the correct direction) and then output it to the Abdomen_joint in my topic /desired_joint_states. The intent is that when I command a pitch of the body the abdomen will move the same amount in the opposite direction.

This is my first attempt at writing code from scratch, up to this point I have been modifying the syropod_highlevel_controller from CSIRO Robotics to support 8 legs.

After a bit of reading I have come up with this... But I'm not sure this is correct and I'm not sure if I am correctly referencing the msg arrays.

void StateController::publishAbdomenState(void)
{
  geometry_msgs::Twist msg;
  sensor_msgs::JointState joint_state_msg;
  Eigen::Quaterniond rotation = model_->getCurrentPose().rotation_;
  msg.angular.y = quaternionToEulerAngles(rotation)[1];

//    if (params_.individual_control_interface.data)
//    {
//      std::shared_ptr<Joint> joint = Abdomen_joint;
//      std_msgs::Float64 position_command_msg;
//      position_command_msg.data = -msg.angular.y;
//      joint->desired_position_publisher_.publish(position_command_msg);
//    }

    if (params_.combined_control_interface.data)
    {
      joint_state_msg.name = "Abdomen_joint";
      joint_state_msg.position = -msg.angular.y;
      desired_joint_state_publisher_.publish(joint_state_msg);
    }
}

When I try to catkin build it get the following error...

Errors     << syropod_highlevel_controller:make /home/robdome/shelob_ws/logs/syropod_highlevel_controller/build.make.026.log
/home/robdome/shelob_ws/src/syropod_highlevel_controller/src/state_controller.cpp: In member function ‘void StateController::publishAbdomenState()’:
/home/robdome/shelob_ws/src/syropod_highlevel_controller/src/state_controller.cpp:948:30: error: no match for ‘operator=’ (operand types are ‘sensor_msgs::JointState_<std::allocator<void> >::_name_type’ {aka ‘std::vector<std::__cxx11::basic_string<char> >’} and ‘const char [14]’)
  948 |       joint_state_msg.name = "Abdomen_joint";
      |                              ^~~~~~~~~~~~~~~
In file included from /usr/include/c++/9/vector:72,
                 from /usr/include/c++/9/functional:62,
                 from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13,
                 from /usr/include/c++/9/algorithm:71,
                 from /usr/include/boost/math/tools/config.hpp:18,
                 from /usr/include/boost/math/special_functions/round.hpp:13,
                 from /opt/ros/noetic/include/ros/time.h:58,
                 from /opt/ros/noetic/include/ros/ros.h:38,
                 from /home/robdome/shelob_ws/src/syropod_highlevel_controller/include/syropod_highlevel_controller/standard_includes.h:12,
                 from /home/robdome/shelob_ws/src/syropod_highlevel_controller/include/syropod_highlevel_controller/state_controller.h:12,
                 from /home/robdome/shelob_ws/src/syropod_highlevel_controller/src/state_controller.cpp:9:
/usr/include/c++/9/bits/vector.tcc:198:5: note: candidate: ‘std::vector<_Tp, _Alloc>& std::vector<_Tp, _Alloc>::operator=(const std::vector<_Tp, _Alloc>&) [with _Tp = std::__cxx11::basic_string<char>; _Alloc = std::allocator<std::__cxx11::basic_string<char> >]’
  198 |     vector<_Tp, _Alloc>::
      |     ^~~~~~~~~~~~~~~~~~~
/usr/include/c++/9/bits/vector.tcc:199:42: note:   no known conversion for argument 1 from ‘const char [14]’ to ‘const std::vector<std::__cxx11::basic_string<char> >&’
  199 |     operator=(const vector<_Tp, _Alloc>& __x)
      |               ~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~
In file included from /usr/include/c++/9/vector:67,
                 from /usr/include/c++/9/functional:62,
                 from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13,
                 from /usr/include/c++/9/algorithm:71,
                 from /usr/include/boost/math/tools/config.hpp:18,
                 from /usr/include/boost/math/special_functions/round.hpp:13,
                 from /opt/ros/noetic/include/ros/time.h:58,
                 from /opt/ros/noetic/include/ros ...
(more)
edit retag flag offensive close merge delete

Comments

1

Is there a missing semicolon when you set the name, or is that just a typo in this post?

Per Edwardsson gravatar image Per Edwardsson  ( 2022-06-01 01:33:03 -0500 )edit

You're right Per Edwardsson I for got the semicolon, I fixed the post.

lutinplunder gravatar image lutinplunder  ( 2022-06-01 04:55:38 -0500 )edit

joint_state_msg.name is a vector of string not a string. So you need to add a string to the vector rather than assign a string to the vector. joint_state_msg.name.push_back("Abdomen_joint");

GuillaumeB gravatar image GuillaumeB  ( 2022-06-09 05:50:40 -0500 )edit