Need help with coding [closed]

edit

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.

//
// Created by robdome on 5/30/22.
//
#include "shelob_syropod/Abdomen.h"

class SubscribeAndPublish
{
public:
    SubscribeAndPublish()
    {
        //Topic you want to publish
        pub_ = n.advertise<sensor_msgs::JointState>("desired_joint_states", 1);

        //Topic you want to subscribe
        sub_ = n_.subscribe("shc/pose", 1000, &SubscribeAndPublish::callback, this);
    }

    void callback(const geometry_msgs::Twist& input)
    {
        sensor_msgs::JointState output;
          output.Abdomen_joint.position = input.angular.y * -1
        pub_.publish(output);
    }

private:
    ros::NodeHandle n_;
    ros::Publisher pub_;
    ros::Subscriber sub_;

};//End of class SubscribeAndPublish

int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "subscribe_and_publish");

    //Create an object of class SubscribeAndPublish that will take care of everything
    SubscribeAndPublish SAPObject;

    ros::spin();

    return 0;
}