Need help with coding [closed]
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;
}