控制真实机械臂
-
@小鱼 鱼哥，我现在使用舵机组成的机械臂，想用moveit控制，下面是我的arduino代码
#include <Servo.h>
#include <ros.h>
#include <sensor_msgs/JointState.h>
ros::NodeHandle nh;
Servo Joint1Servo;
Servo Joint2Servo;
Servo Joint3Servo;
Servo Joint4Servo;
const int servo_pins[] = {2, 3, 4, 5};
// 在Arduino环境中，我们通常使用固定大小数组代表关节名称和位置。
char *joint_names[] = {"shoulder_pan", "shoulder_lift", "elbow","wrist_flex"};
float joint_positions[4];
float joint_actual_positions[4]; // 新增数组存储实际读取的关节位置
sensor_msgs::JointState joint_state_msg;
ros::Publisher joint_state_pub("joint_states", &joint_state_msg);
void servo_cb(const sensor_msgs::JointState& msg);ros::Subscriber<sensor_msgs::JointState> trajectory_subscriber("/joint_states", servo_cb);
void servo_cb(const sensor_msgs::JointState& msg) {
if (msg.position_length != 4 || msg.name_length != 4) {
Serial.println("Received JointState message with incorrect number of joints.");
return;
}
for (int i = 0; i < 4; i++) {
joint_positions[i] = msg.position[i] * 180.0 / M_PI; // 将弧度转为角度
int angle = joint_positions[i];
switch(i){
case 0: Joint1Servo.write(angle); break;
case 1: Joint2Servo.write(angle); break;
case 2: Joint3Servo.write(angle); break;
case 3: Joint4Servo.write(angle); break;
}
}
}
void readJointPositions() {
for(int i = 0; i < 4; i++){
joint_actual_positions[i] = Joint1Servo.read();
}
}
void setup() {
nh.initNode();
nh.advertise(joint_state_pub);
nh.subscribe(trajectory_subscriber);
for(int i = 0; i < 4; i++){
joint_positions[i] = 90.0; // 初始化为90度，或者其他中位值
joint_actual_positions[i] = 90.0; // 初始化实际位置也为90度
}
Joint1Servo.attach(servo_pins[0]);
Joint2Servo.attach(servo_pins[1]);
Joint3Servo.attach(servo_pins[2]);
Joint4Servo.attach(servo_pins[3]);
// 初始化JointState消息
joint_state_msg.name = joint_names;
joint_state_msg.position = joint_actual_positions;
joint_state_msg.name_length = 4;
joint_state_msg.position_length = 4;
}
void loop() {
readJointPositions();
joint_state_msg.header.stamp = nh.now();
joint_state_pub.publish(&joint_state_msg);
nh.spinOnce();
delay(10);
}是有什么问题吗，我的机械臂并没有动，moveit部分虚拟控制器关了，
rostopic echo /arm_controller/follow_joint_trajectory/goal
WARNING: no messages received and simulated time is active.
Is /clock being published?
header:
seq: 10
stamp:
secs: 2758
nsecs: 188000000
frame_id: ''
goal_id:
stamp:
secs: 2758
nsecs: 188000000
id: "/move_group-11-2758.188000000"
goal:
trajectory:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "base_link"
joint_names:
- elbow
- shoulder_lift
- shoulder_pan
- wrist_flex
points:
-
positions: [5.664923982706682e-05, 0.5233865867053824, -0.33860453476439023, -0.5672284925053317]
velocities: [0.0, 0.0, 0.0, 0.0]
accelerations: [0.0, 0.0, -0.00406710291139345, 0.0]
/joint_states也能打印出消息，还需要什么操作才能控制机械臂运动呢 -
@2692784212 老哥解决问题没有？机械臂开发好没有？