# define _USE_MATH_DEFINES
#include "ros/ros.h"
#include "std_msgs/Float64.h" //使用的消息类型为sensor_msgs::JointState
#include <math.h>
#include <string>
#include<iostream>
int main(int argc, char** argv)
{
double data;
int x=-1;
ros::init(argc, argv, "joint_state_publisher_sy");
ros::NodeHandle nh;
ros::Publisher joints_std_msgs[9];
std_msgs::Float64 joint_msg;
joints_std_msgs[1] = nh.advertise<std_msgs::Float64>("/toydog/joint1_position_controller/command", 1000);
joints_std_msgs[2] = nh.advertise<std_msgs::Float64>("/toydog/joint2_position_controller/command", 1000);
joints_std_msgs[3] = nh.advertise<std_msgs::Float64>("/toydog/joint3_position_controller/command", 1000);
joints_std_msgs[4] = nh.advertise<std_msgs::Float64>("/toydog/joint4_position_controller/command", 1000);
joints_std_msgs[5] = nh.advertise<std_msgs::Float64>("/toydog/joint5_position_controller/command", 1000);
joints_std_msgs[6] = nh.advertise<std_msgs::Float64>("/toydog/joint6_position_controller/command", 1000);
joints_std_msgs[7] = nh.advertise<std_msgs::Float64>("/toydog/joint7_position_controller/command", 1000);
joints_std_msgs[8] = nh.advertise<std_msgs::Float64>("/toydog/joint8_position_controller/command", 1000);
while(1){
std::cout<<"请输入控制的关节(1-8:Joint,9:ALL):";
std::cin >> x;
if(x<1||x>9)
break;
std::cout << "请输入data:";//angular
std::cin >> data;
joint_msg.data = data * M_PI / 180;
if(x==9){
for (int i = 1; i <= 8;i++)
joints_std_msgs[i].publish(joint_msg);
ROS_INFO("publish success");
}
else{
joints_std_msgs[x].publish(joint_msg);
ROS_INFO("publish success");
}
}
return 0;
}
ROS cpp code
于 2022-11-04 15:46:56 首次发布