전체 흐름
ros2 설치
colcon build 설치
miniconda 설치
Miniconda 설치
error 발생
미니콘다 설치를 다한 후 shell 를 재실행 하여도 miniconda 실행 안됨
> $~/miniconda3/bin/conda init
코드 실행하여 해결
conda create -n exo39 python=3.9
->conda create -n exo310 python=3.10
python 버전 3.9 로 하면 error 발생
ROS2 환경 세팅
ROS2 설치 후 colcon build 도 설치 진행해야한다.
이후 pip install catkin_pkg
$colcon build –symlink-install 의 결과
dynamixel ROS2 환경
error 발생 No module named ’em’
pip install empy
pip install numpy
pip install lark
에러가 발생하고 새로 설치해준 순서
sudo chmod a+rw /dev/ttyUSB0
항상 해주기
sudo usermod -aG dialout $USER
를 하면 매번 위에 포트에 권한 주는 코드를 입력하지 않아도 됨
dmesg | grep ttyUSB
로 연결된 포트 확인 가능 USB0 이 아닐수도 있으므로
항상 source install/setup.bash 잊지 말기
. ~/dynamixel_ws/install/setup.bash
ros2 run test_dynamixel test_move /dev/ttyUSB0
모터 명령 날리는(pub) 코드 &받아서(sub) 모터 제어하는 코드
cd ~/dynamixel_ws/src
ros2 pkg create –build-type ament_python gen_pos –dependencies rclpy std_msgs
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
# cmd_deg 토픽으로 각도(도)를 퍼블리시
# 파라미터:
# - angles_deg: "0,90,180,270,0" 형태의 문자열
# - rate_hz: 퍼블리시 주기(Hz)
class GenPosNode(Node):
def __init__(self):
super().__init__('gen_pos_node')
self.declare_parameter('angles_deg', '0,90,180,0')
self.declare_parameter('rate_hz', 1.0)
angles_str = self.get_parameter('angles_deg').get_parameter_value().string_value
self.angles = []
for tok in angles_str.split(','):
tok = tok.strip()
if tok:
try:
self.angles.append(float(tok))
except ValueError:
self.get_logger().warn(f'Invalid angle token "{tok}" ignored.')
if not self.angles:
self.angles = [0.0]
self.rate_hz = float(self.get_parameter('rate_hz').value)
self.pub = self.create_publisher(Float64, 'cmd_deg', 10)
period = 1.0 / max(self.rate_hz, 1e-3)
self.idx = 0
self.timer = self.create_timer(period, self.tick)
self.get_logger().info(f'Publishing angles (deg): {self.angles} at {self.rate_hz} Hz on /cmd_deg')
def tick(self):
msg = Float64()
msg.data = self.angles[self.idx]
self.pub.publish(msg)
self.get_logger().info(f'Publish deg: {msg.data:.3f}')
self.idx = (self.idx + 1) % len(self.angles)
def main():
rclpy.init()
node = GenPosNode()
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()전체 코드
dynamixel 제어 cpp 코드
// test_pos8.cpp — 초기값(하드코딩) + 들어온 각도오프셋(°) 합산 → GOAL_POSITION
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/float64_multi_array.hpp"
#include "dynamixel_sdk/dynamixel_sdk.h"
#include <cmath>
#include <memory>
using namespace dynamixel;
using std::placeholders::_1;
#define ADDR_OPERATING_MODE 11
#define ADDR_TORQUE_ENABLE 64
#define ADDR_GOAL_POSITION 116
#define PROTOCOL_VERSION 2.0
static inline int deg_to_tick(double deg) {
return static_cast<int>(std::lround(deg * 4096.0 / 360.0)); // 180deg ≈ 2048 tick
}
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("test_pos8_min");
// 포트/보레이트/ID
auto port = PortHandler::getPortHandler("/dev/ttyUSB0");
auto packet = PacketHandler::getPacketHandler(PROTOCOL_VERSION);
if (!port->openPort()) { RCLCPP_FATAL(node->get_logger(), "openPort failed"); return 1; }
if (!port->setBaudRate(115200)) { RCLCPP_FATAL(node->get_logger(), "setBaudRate failed"); return 1; }
static const uint8_t IDS[] = {4,5,6,7,8,9,10,11};
static const size_t N = sizeof(IDS)/sizeof(IDS[0]);
// 초기 각도: 4~7: 90,180,180,180 → 1024,2048,2048,2048
// 8~11: 180,180,180,180 → 2048,2048,2048,2048
static const int INIT_PULSES[N] = {1024, 2048, 2048, 2048, 2048, 2048, 2048, 2048};
uint8_t dxl_error = 0;
for (size_t i = 0; i < N; ++i) {
packet->write1ByteTxRx(port, IDS[i], ADDR_OPERATING_MODE, 3, &dxl_error); // position mode
packet->write1ByteTxRx(port, IDS[i], ADDR_TORQUE_ENABLE, 1, &dxl_error); // torque on
}
auto gsw = std::make_shared<GroupSyncWrite>(port, packet, ADDR_GOAL_POSITION, 4);
auto send_raw = [&](const int *vals) {
gsw->clearParam();
for (size_t i = 0; i < N; ++i) {
const int v = vals[i];
uint8_t param[4] = {
DXL_LOBYTE(DXL_LOWORD(v)), DXL_HIBYTE(DXL_LOWORD(v)),
DXL_LOBYTE(DXL_HIWORD(v)), DXL_HIBYTE(DXL_HIWORD(v))
};
gsw->addParam(IDS[i], param);
}
gsw->txPacket(); // 에러 체크 생략(최소 구현)
};
// 1) 초기 명령 전송
send_raw(INIT_PULSES);
RCLCPP_INFO(node->get_logger(), "Initial pulses sent (IDs 4..11).");
// 2) 구독: 각도 오프셋(°) → tick 변환 후 초기값에 더해 전송
auto sub = node->create_subscription<std_msgs::msg::Float64MultiArray>(
"/cmd_degs", 10,
[&, node](const std_msgs::msg::Float64MultiArray::SharedPtr msg) {
if (msg->data.size() < N) {
RCLCPP_WARN(node->get_logger(), "need %zu deg offsets; got %zu", N, msg->data.size());
return;
}
int goals[N];
for (size_t i = 0; i < N; ++i) {
const double deg_off = msg->data[i]; // -10 ~ +10 deg (권장)
const int tick_off = deg_to_tick(deg_off); // 약 ±114 tick
goals[i] = INIT_PULSES[i] + tick_off; // 클램프 없음(요청사항)
}
send_raw(goals);
});
rclcpp::spin(node);
for (size_t i = 0; i < N; ++i)
packet->write1ByteTxRx(port, IDS[i], ADDR_TORQUE_ENABLE, 0, &dxl_error);
port->closePort();
rclcpp::shutdown();
return 0;
}
dynamixel 제어하기 위한 제어 입력(sin)생성하는 코드
# gen_pos.py — 첫 번째 모터만 사인파(±10 deg), 나머지 0 deg
import math
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
class GenPosNode(Node):
def __init__(self):
super().__init__('gen_pos_node')
# 파라미터: 진폭(도), 주파수(Hz), 발행주기(Hz), 모터 수
self.declare_parameter('amp_deg', 10.0) # -10 ~ +10 deg
self.declare_parameter('freq_hz', 0.5) # 0.5 Hz
self.declare_parameter('rate_hz', 50.0) # 50 Hz 발행
self.declare_parameter('num_motors', 8) # 모터 개수(기본 8개: ID 4~11)
self.amp = float(self.get_parameter('amp_deg').value)
self.freq = float(self.get_parameter('freq_hz').value)
self.rate = float(self.get_parameter('rate_hz').value)
self.nmot = int(self.get_parameter('num_motors').value)
self.pub = self.create_publisher(Float64MultiArray, 'cmd_degs', 10)
period = 1.0 / max(self.rate, 1e-3)
self.t0 = self.get_clock().now()
self.timer = self.create_timer(period, self.tick)
self.get_logger().info(
f'gen_pos: first motor = sin(2π*{self.freq}*t)*{self.amp} deg, others=0 deg '
f'-> /cmd_degs (len={self.nmot}) at {self.rate} Hz'
)
def tick(self):
t = (self.get_clock().now() - self.t0).nanoseconds * 1e-9
v0 = self.amp * math.sin(2.0 * math.pi * self.freq * t) # 첫 모터(deg)
msg = Float64MultiArray()
msg.data = [0.0] * self.nmot
# msg.data[0] = v0
for i in range(self.nmot):
msg.data[i]=v0
self.pub.publish(msg)
def main():
rclpy.init()
node = GenPosNode()
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
0 Comments