diff --git a/README.md b/README.md index 3d7fb6737d4f8aef1feba7d30fb139dbc81a96b5..f91beb8be2cd3179259d985f8046f8bec501c946 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,11 @@ The coarm_ros2 repository allowing efficient control of the Coarm robots within Coarm机械臂ROS2开发包，支持Coarm机械臂在ROS2下的集成控制。 +### Update v1.0.17 +1. 修复hardware初始化时setup_motion提前问题和初始位置始终为0问题 +2. 修改control的运动点位速度和加速度受moveit变化控制，movej接收多个轨迹点位 +3. 修改send_point服务接口下控制延时，ros服务启动参数 + ### Update v1.0.16 1. 新增coarm_omnipicker_moveit_config包 2. 修复eye_to_hand相关错误 diff --git a/coarm_driver/coarm_driver/server.py b/coarm_driver/coarm_driver/server.py index 1fd5c129a34488804fb482852f3a0357ba4cb9c7..3755860ac7d140045ccd5fec4a051a70b519d62a 100644 --- a/coarm_driver/coarm_driver/server.py +++ b/coarm_driver/coarm_driver/server.py @@ -37,8 +37,8 @@ class StatePublisher(Node): # Create publisher self.joints_publisher = self.create_publisher(Joints, '/coarm_server/joints', 10) self.pose_publisher = self.create_publisher(Pose, '/coarm_server/pose', 10) - # Create publish timer - self.timer = self.create_timer(0.01, self.publish_callback) + # Create publish timer (50hz) + self.timer = self.create_timer(0.02, self.publish_callback) def publish_callback(self): joints = Joints() @@ -328,11 +328,12 @@ class SendPointService(Node): duration = list(request.duration) count = list(request.count) last = list(request.last) - self.get_logger().info('Incoming send_point request with ' - 'joint_ids: %s, position: %s, ' - 'velocity: %s, duration: %s, ' - 'count: %s, last: %s' - % (joint_ids, position, velocity, duration, count, last)) + # if (sum(velocity) != 0): + # self.get_logger().info('Incoming send_point request with ' + # 'joint_ids: %s, position: %s, ' + # 'velocity: %s, duration: %s, ' + # 'count: %s, last: %s' + # % (joint_ids, position, velocity, duration, count, last)) position = {str(joint_ids[i]): float(position[i]) for i in range(len(joint_ids))} velocity = {str(joint_ids[i]): float(velocity[i]) for i in range(len(joint_ids))} duration = {str(joint_ids[i]): float(duration[i]) for i in range(len(joint_ids))} @@ -350,7 +351,9 @@ class SendPointService(Node): for status in buffer_status: value &= status if value > 0: - time.sleep(PVT_DURATION) + # pvt_motion_client 测试时需要 + # time.sleep(PVT_DURATION) + pass else: break response.status = CONTROLLER_OK diff --git a/coarm_driver/config/coarm_server.yaml b/coarm_driver/config/coarm_server.yaml index fb06405a517087578c8db17b6c11b269cadf5093..330f47c0bf201eac66919cd9dab8889dff89c69b 100644 --- a/coarm_driver/config/coarm_server.yaml +++ b/coarm_driver/config/coarm_server.yaml @@ -1,4 +1,4 @@ /get_parameters: ros__parameters: - robot_mode: 0 - robot_name: COARM \ No newline at end of file + robot_mode: 1 + robot_name: coarm \ No newline at end of file diff --git a/coarm_hardware/src/coarm_hardware_interface.cpp b/coarm_hardware/src/coarm_hardware_interface.cpp index cfd48857cbd074475c2126e647e268f62a061cb8..d6c32fd270746fa732f034e681f19b02574f2df5 100644 --- a/coarm_hardware/src/coarm_hardware_interface.cpp +++ b/coarm_hardware/src/coarm_hardware_interface.cpp @@ -119,10 +119,13 @@ namespace coarm_hardware } RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), "Send request to CoarmServer setup pvt motion"); auto setup_motion_result = coarmlibs_setup_motion_client_->async_send_request(setup_motion_request); - std::future_status setup_motion_status = setup_motion_result.wait_for(2s); - if (setup_motion_status == std::future_status::ready) { - RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), - "Received CoarmServer setup_motion service response"); + while(rclcpp::ok()){ + std::future_status setup_motion_status = setup_motion_result.wait_for(2s); + if (setup_motion_status == std::future_status::ready) { + RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), + "Received CoarmServer setup_motion service response"); + break; + } } point_count_ = (point_count_ + 1) & MAX_COUNT; RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), "Coarm Hardware Sucessfully configured!"); @@ -139,8 +142,8 @@ namespace coarm_hardware { if (std::isnan(hw_position_states_[i])) { - hw_position_states_[i] = 0; - hw_position_cmds_[i] = 0; + hw_position_states_[i] = position_states_[i]; + hw_position_cmds_[i] = position_states_[i]; } else { @@ -227,13 +230,13 @@ namespace coarm_hardware for (size_t i = 0; i < hw_position_cmds_.size(); i++) { send_point_request->joint_ids.push_back(std::to_string(i + 1)); - send_point_request->position.push_back(next_position_cmds_[i]); + send_point_request->position.push_back(position_cmds_[i]); // Velocity = (distance) / (duration) between last position and next position -// RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), -// "%f, %f, %f, %f", next_position_cmds_[i], last_position_cmds_[i], duration_, next_duration_); - double velocity = (next_position_cmds_[i] - last_position_cmds_[i]) / (0.01 + 0.01); + // RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), + // "%f, %f, %f, %f", next_position_cmds_[i], last_position_cmds_[i], duration_, next_duration_); + double velocity = (next_position_cmds_[i] - last_position_cmds_[i]) / (duration_ + next_duration_); send_point_request->velocity.push_back(velocity); - send_point_request->duration.push_back(0.01); + send_point_request->duration.push_back(0.02); send_point_request->count.push_back(point_count_); send_point_request->last.push_back(false); } diff --git a/coarm_planner/src/coarm_control.cpp b/coarm_planner/src/coarm_control.cpp index 5fb4ca46d64ea5d7001ad4f7a0a0ffb2707c788d..5d2e0b3aded279a3cfbb3aa627c5ed9157c335b9 100644 --- a/coarm_planner/src/coarm_control.cpp +++ b/coarm_planner/src/coarm_control.cpp @@ -49,6 +49,11 @@ public: !coarmlibs_movej_client_->wait_for_service(1s) || !coarmlibs_stop_client_->wait_for_service(1s)) { + if (!rclcpp::ok()) + { + std::cerr << "[ERROR]\tInterrupted while waiting for the service. Exiting." << std::endl; + return; + } RCLCPP_INFO(this->get_logger(), "CoarmServer not available, waiting again..."); } RCLCPP_INFO(this->get_logger(), "CoarmControl initialized"); @@ -168,14 +173,96 @@ private: { // Call CoarmServer MoveJ service and send last trajectory point to MoveJ service auto movej_request = std::make_shared(); - for (size_t i = 0; i < joint_names_.size(); i++) + // float max_vel = 0; + // for (size_t i = 1; i < point_num - 1; i++){ + // for (size_t j = 0; j < joint_names_.size(); j++) + // { + // float vel = std::fabs(torso_goal->trajectory.points[i].velocities[joint_match[j]] * 180 / PI); + // if (max_vel < 20.0) + // { + // max_vel = 20.0; + // continue; + // } + // if (max_vel > vel) + // { + // continue; + // } + // max_vel = vel; + // } + // } + // RCLCPP_INFO(this->get_logger(), "Accepted joint vel: %f ", max_vel); + float max_acc = 0; + for (size_t i = 1; i < point_num - 1; i++){ + for (size_t j = 0; j < joint_names_.size(); j++) + { + float acc = std::fabs(torso_goal->trajectory.points[i].accelerations[joint_match[j]] * 180 / PI); + if (max_acc < 30.0) + { + max_acc = 30.0; + continue; + } + if (max_acc > acc) + { + continue; + } + max_acc = acc; + } + } + for (size_t i = 2; i < point_num - 3; i += 2){ + movej_request->joint_ids.clear(); + movej_request->joints.clear(); + for (size_t j = 0; j < joint_names_.size(); j++) + { + movej_request->joint_ids.push_back(std::to_string(j + 1)); + movej_request->joints.push_back( + torso_goal->trajectory.points[i].positions[joint_match[j]] * 180 / PI); + } + float max_vel = 0; + for (size_t j = 0; j < joint_names_.size(); j++) + { + float vel = std::fabs(torso_goal->trajectory.points[i].velocities[joint_match[j]] * 180 / PI); + if (max_vel < 10.0) + { + max_vel = 10.0; + continue; + } + if (max_vel > vel) + { + continue; + } + max_vel = vel; + } + movej_request->velocity = max_vel; + movej_request->acceleration = max_acc; + movej_request->block = false; + movej_request->advance = false; + coarmlibs_movej_client_->async_send_request(movej_request); + } + movej_request->joint_ids.clear(); + movej_request->joints.clear(); + for (size_t j = 0; j < joint_names_.size(); j++) { - movej_request->joint_ids.push_back(std::to_string(i + 1)); + movej_request->joint_ids.push_back(std::to_string(j + 1)); movej_request->joints.push_back( - torso_goal->trajectory.points[point_num - 1].positions[joint_match[i]] * 180 / PI); + torso_goal->trajectory.points[point_num - 1].positions[joint_match[j]] * 180 / PI); } - movej_request->velocity = 50.0; - movej_request->acceleration = 100.0; + float max_vel = 0; + for (size_t j = 0; j < joint_names_.size(); j++) + { + float vel = std::fabs(torso_goal->trajectory.points[point_num - 1].velocities[joint_match[j]] * 180 / PI); + if (max_vel < 10.0) + { + max_vel = 10.0; + continue; + } + if (max_vel > vel) + { + continue; + } + max_vel = vel; + } + movej_request->velocity = max_vel; + movej_request->acceleration = max_acc; movej_request->block = true; movej_request->advance = false; auto movej_result = coarmlibs_movej_client_->async_send_request(movej_request); @@ -205,14 +292,50 @@ private: { // Call CoarmServer MoveJoints service and send last trajectory point to MoveJoints service auto move_joints_request = std::make_shared(); + float max_vel = 0; + for (size_t i = 1; i < point_num - 1; i++){ + for (size_t j = 0; j < joint_names_.size(); j++) + { + float vel = std::fabs(torso_goal->trajectory.points[i].velocities[joint_match[j]] * 180 / PI); + if (max_vel < 20.0) + { + max_vel = 20.0; + continue; + } + if (max_vel > vel) + { + continue; + } + max_vel = vel; + } + } + RCLCPP_INFO(this->get_logger(), "Accepted joint vel: %f ", max_vel); + float max_acc = 0; + for (size_t i = 1; i < point_num - 1; i++){ + for (size_t j = 0; j < joint_names_.size(); j++) + { + float acc = std::fabs(torso_goal->trajectory.points[i].accelerations[joint_match[j]] * 180 / PI); + if (max_acc < 40.0) + { + max_acc = 40.0; + continue; + } + if (max_acc > acc) + { + continue; + } + max_acc = acc; + } + } + RCLCPP_INFO(this->get_logger(), "Accepted joint acc: %f ", max_acc); for (size_t i = 0; i < joint_names_.size(); i++) { move_joints_request->joint_ids.push_back(std::to_string(i + 1)); move_joints_request->joints.push_back( torso_goal->trajectory.points[point_num - 1].positions[joint_match[i]] * 180 / PI); } - move_joints_request->velocity = 50.0; - move_joints_request->acceleration = 100.0; + move_joints_request->velocity = max_vel; + move_joints_request->acceleration = max_acc; move_joints_request->block = true; move_joints_request->advance = false; auto move_joints_result = coarmlibs_move_joints_client_->async_send_request(move_joints_request);