ROS cpp code

# 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;
}

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值