39 #include "std_msgs/String.h" 40 #include "dynamixel_sdk_examples/GetPosition.h" 41 #include "dynamixel_sdk_examples/SetPosition.h" 47 #define ADDR_TORQUE_ENABLE 64 48 #define ADDR_GOAL_POSITION 116 49 #define ADDR_PRESENT_POSITION 132 52 #define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series. 55 #define DXL1_ID 1 // DXL1 ID 56 #define DXL2_ID 2 // DXL2 ID 57 #define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series 58 #define DEVICE_NAME "/dev/ttyUSB0" // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command 64 dynamixel_sdk_examples::GetPosition::Request & req,
65 dynamixel_sdk_examples::GetPosition::Response & res)
67 uint8_t dxl_error = 0;
78 ROS_INFO(
"getPosition : [ID:%d] -> [POSITION:%d]", req.id, position);
79 res.position = position;
82 ROS_INFO(
"Failed to get position! Result: %d", dxl_comm_result);
89 uint8_t dxl_error = 0;
93 uint32_t position = (
unsigned int)msg->position;
100 ROS_INFO(
"setPosition : [ID:%d] [POSITION:%d]", msg->id, msg->position);
102 ROS_ERROR(
"Failed to set position! Result: %d", dxl_comm_result);
106 int main(
int argc,
char ** argv)
108 uint8_t dxl_error = 0;
120 ROS_ERROR(
"Failed to set the baudrate!");
138 ros::init(argc, argv,
"read_write_node");
static PortHandler * getPortHandler(const char *port_name)
#define ADDR_GOAL_POSITION
virtual bool openPort()=0
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
PacketHandler * packetHandler
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
#define ADDR_TORQUE_ENABLE
PortHandler * portHandler
virtual void closePort()=0
int main(int argc, char **argv)
bool getPresentPositionCallback(dynamixel_sdk_examples::GetPosition::Request &req, dynamixel_sdk_examples::GetPosition::Response &res)
virtual int write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error=0)=0
static PacketHandler * getPacketHandler(float protocol_version=2.0)
#define ADDR_PRESENT_POSITION
void setPositionCallback(const dynamixel_sdk_examples::SetPosition::ConstPtr &msg)
virtual int write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error=0)=0
virtual int read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error=0)=0
virtual bool setBaudRate(const int baudrate)=0