From b9854798d500247cb3948c0badd3f4a649e6951f Mon Sep 17 00:00:00 2001 From: Felix Goldau Date: Wed, 16 Nov 2022 17:03:49 +0100 Subject: [PATCH 1/7] Added []-operators to AngularInfo --- kinova_driver/include/kinova/KinovaTypes.h | 25 ++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/kinova_driver/include/kinova/KinovaTypes.h b/kinova_driver/include/kinova/KinovaTypes.h index c09212c61..3e83cdf03 100644 --- a/kinova_driver/include/kinova/KinovaTypes.h +++ b/kinova_driver/include/kinova/KinovaTypes.h @@ -468,6 +468,31 @@ struct AngularInfo Actuator6 = 0.0f; Actuator7 = 0.0f; } + + float& operator [](const int idx){ + switch (idx) + { + case 0: return Actuator1; + case 1: return Actuator2; + case 2: return Actuator3; + case 3: return Actuator4; + case 4: return Actuator5; + case 5: return Actuator6; + case 6: return Actuator7; + } + } + float operator [](const int idx) const{ + switch (idx) + { + case 0: return Actuator1; + case 1: return Actuator2; + case 2: return Actuator3; + case 3: return Actuator4; + case 4: return Actuator5; + case 5: return Actuator6; + case 6: return Actuator7; + } + } }; /** @brief This data structure holds values in an cartesian control context. From cc241857d0b90e8c11f91a93eeffe82ad02f5f3f Mon Sep 17 00:00:00 2001 From: Felix Goldau Date: Wed, 16 Nov 2022 17:04:15 +0100 Subject: [PATCH 2/7] Fixed joint traj goal/velocity constraint checks --- .../joint_trajectory_action_server.cpp | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp b/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp index 1f082626b..26f22612d 100644 --- a/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp +++ b/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp @@ -236,10 +236,12 @@ void JointTrajectoryActionController::controllerStateCB(const control_msgs::Foll { // Checks that we have ended inside the goal constraints bool inside_goal_constraints = true; - for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i) + bool inside_velocity_constraints = true; + for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints && inside_velocity_constraints; ++i) { // computing error from goal pose double abs_error = fabs(msg->actual.positions[i] - current_traj_.points[last].positions[i]); + abs_error = abs_error - 2*M_PI*std::floor((abs_error+M_PI)/(2*M_PI)); // fix rotation to [-PI,PI) double goal_constraint = goal_constraints_[msg->joint_names[i]]; if (goal_constraint >= 0 && abs_error > goal_constraint) inside_goal_constraints = false; @@ -247,10 +249,10 @@ void JointTrajectoryActionController::controllerStateCB(const control_msgs::Foll if ( !(msg->desired.velocities.empty()) && (fabs(msg->desired.velocities[i]) < 1e-6) ) { if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_) - inside_goal_constraints = false; + inside_velocity_constraints = false; } } - if (inside_goal_constraints) + if (inside_goal_constraints && inside_velocity_constraints) { active_goal_.setSucceeded(); has_active_goal_ = false; @@ -262,7 +264,17 @@ void JointTrajectoryActionController::controllerStateCB(const control_msgs::Foll } else { - ROS_WARN("Aborting because we wound up outside the goal constraints"); + ROS_WARN("Aborting because we wound up outside the %s constraints", inside_velocity_constraints?"goal":"velocity"); + for(int j = 0; j<7; j++){ + double abs_error; + if(!inside_goal_constraints){ + abs_error = fabs(msg->actual.positions[j] - current_traj_.points[last].positions[j]); + abs_error = abs_error - 2*M_PI*std::floor((abs_error+M_PI)/(2*M_PI)); // fix rotation to [-PI,PI) + } else { + abs_error = fabs(msg->actual.velocities[j] - stopped_velocity_tolerance_); + } + ROS_WARN_STREAM("Error " << j << ": " << abs_error); + } active_goal_.setAborted(); has_active_goal_ = false; } From 1ac82488cc6a62dde4ae25184e040f2a3cf720c0 Mon Sep 17 00:00:00 2001 From: Felix Goldau Date: Wed, 16 Nov 2022 17:09:49 +0100 Subject: [PATCH 3/7] Removed Debug outputs and integrated []-operators --- .../kinova_joint_trajectory_controller.cpp | 128 ++++-------------- 1 file changed, 23 insertions(+), 105 deletions(-) diff --git a/kinova_driver/src/kinova_joint_trajectory_controller.cpp b/kinova_driver/src/kinova_joint_trajectory_controller.cpp index 91fae91eb..0fc8502f6 100644 --- a/kinova_driver/src/kinova_joint_trajectory_controller.cpp +++ b/kinova_driver/src/kinova_joint_trajectory_controller.cpp @@ -8,33 +8,23 @@ JointTrajectoryController::JointTrajectoryController(kinova::KinovaComm &kinova_ kinova_comm_(kinova_comm), nh_(n) { - //ROS_DEBUG_STREAM_ONCE("Get in: " << __PRETTY_FUNCTION__); - ros::NodeHandle pn("~"); std::string robot_type; nh_.param("robot_name",prefix_,"j2n6s300"); nh_.param("robot_type",robot_type,"j2n6s300"); number_joint_ =robot_type[3] - '0'; - // Display debug information in teminal - if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { - ros::console::notifyLoggerLevelsChanged(); - } - sub_command_ = nh_.subscribe("trajectory_controller/command", 1, &JointTrajectoryController::commandCB, this); pub_joint_feedback_ = nh_.advertise("trajectory_controller/state", 1); pub_joint_velocity_ = pn.advertise("in/joint_velocity", 2); - traj_frame_id_ = "root"; + traj_frame_id_ = "root"; joint_names_.resize(number_joint_); - //std::cout << "joint names in feedback of trajectory state are: " << std::endl; for (uint i = 0; i(i+1); - std::cout << joint_names_[i] << " "; } - std::cout << std::endl; timer_pub_joint_vel_ = nh_.createTimer(ros::Duration(0.01), &JointTrajectoryController::pub_joint_vel, this, false, false); terminate_thread_ = false; @@ -52,13 +42,10 @@ JointTrajectoryController::JointTrajectoryController(kinova::KinovaComm &kinova_ // counter in the timer to publish joint velocity command: pub_joint_vel() traj_command_points_index_ = 0; - - //ROS_DEBUG_STREAM_ONCE("Get out: " << __PRETTY_FUNCTION__); } JointTrajectoryController::~JointTrajectoryController() { - //ROS_DEBUG_STREAM_ONCE("Get in: " << __PRETTY_FUNCTION__); ROS_WARN("destruction entered!"); { boost::mutex::scoped_lock terminate_lock(terminate_thread_mutex_); @@ -72,35 +59,16 @@ JointTrajectoryController::~JointTrajectoryController() timer_pub_joint_vel_.stop(); thread_update_state_->join(); delete thread_update_state_; - - //ROS_DEBUG_STREAM_ONCE("Get out: " << __PRETTY_FUNCTION__); } void JointTrajectoryController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &traj_msg) { - //ROS_DEBUG_STREAM_ONCE("Get in: " << __PRETTY_FUNCTION__); - bool command_abort = false; -// // if receive new command, clear all trajectory and stop api -// kinova_comm_.stopAPI(); -// if(!kinova_comm_.isStopped()) -// { -// ros::Duration(0.01).sleep(); -// } -// kinova_comm_.eraseAllTrajectories(); - -// kinova_comm_.startAPI(); -// if(kinova_comm_.isStopped()) -// { -// ros::Duration(0.01).sleep(); -// } - traj_command_points_ = traj_msg->points; - ROS_INFO_STREAM("Trajectory controller Receive trajectory with points number: " << traj_command_points_.size()); - + ROS_INFO_STREAM_NAMED("Trajectory controller", "Receive trajectory with points number: " << traj_command_points_.size()); // Map the index in joint_names and the msg std::vector lookup(number_joint_, -1); @@ -139,9 +107,15 @@ void JointTrajectoryController::commandCB(const trajectory_msgs::JointTrajectory command_abort = true; break; } - + // velocity should not be empty + if (traj_command_points_[j].velocities.empty()) + { + ROS_ERROR_STREAM("Velocities in trajectory command cannot be empty at point: " << j); + command_abort = true; + break; + } // if velocity provided, size match - if (!traj_command_points_[j].velocities.empty() && traj_command_points_[j].velocities.size() != number_joint_) + if (traj_command_points_[j].velocities.size() != number_joint_) { ROS_ERROR_STREAM("Velocities at point " << j << " has size " << traj_command_points_[j].velocities.size() << " in trajectory command, which does not match joint number! "); command_abort = true; @@ -153,22 +127,13 @@ void JointTrajectoryController::commandCB(const trajectory_msgs::JointTrajectory return; // store angle velocity command sent to robot -// std::vector kinova_angle_command; kinova_angle_command_.resize(traj_command_points_.size()); for (size_t i = 0; i=6) + for(int actuator = 0; actuator < number_joint_; actuator++) { - kinova_angle_command_[i].Actuator5 = traj_command_points_[i].velocities[4] *180/M_PI; - kinova_angle_command_[i].Actuator6 = traj_command_points_[i].velocities[5] *180/M_PI; - if (number_joint_==7) - kinova_angle_command_[i].Actuator7 = traj_command_points_[i].velocities[6] *180/M_PI; + kinova_angle_command_[i][actuator] = traj_command_points_[i].velocities[actuator] *180/M_PI; } } @@ -176,46 +141,33 @@ void JointTrajectoryController::commandCB(const trajectory_msgs::JointTrajectory double trajectory_duration = traj_command_points_[0].time_from_start.toSec(); durations[0] = trajectory_duration; -// ROS_DEBUG_STREAM("durationsn 0 is: " << durations[0]); for (int i = 1; i=6) - { - traj_feedback_msg_.desired.positions[4] = current_joint_command.Actuators.Actuator5 *M_PI/180; - traj_feedback_msg_.desired.positions[5] = current_joint_command.Actuators.Actuator6 *M_PI/180; - if (number_joint_==7) - traj_feedback_msg_.desired.positions[6] = current_joint_command.Actuators.Actuator7 *M_PI/180; - } - - traj_feedback_msg_.actual.positions[0] = current_joint_angles.Actuator1 *M_PI/180; - traj_feedback_msg_.actual.positions[1] = current_joint_angles.Actuator2 *M_PI/180; - traj_feedback_msg_.actual.positions[2] = current_joint_angles.Actuator3 *M_PI/180; - traj_feedback_msg_.actual.positions[3] = current_joint_angles.Actuator4 *M_PI/180; - if (number_joint_>=6) - { - traj_feedback_msg_.actual.positions[4] = current_joint_angles.Actuator5 *M_PI/180; - traj_feedback_msg_.actual.positions[5] = current_joint_angles.Actuator6 *M_PI/180; - if (number_joint_==7) - traj_feedback_msg_.actual.positions[6] = current_joint_angles.Actuator7 *M_PI/180; - } - - traj_feedback_msg_.actual.velocities[0] = current_joint_velocity.Actuator1 *M_PI/180; - traj_feedback_msg_.actual.velocities[1] = current_joint_velocity.Actuator2 *M_PI/180; - traj_feedback_msg_.actual.velocities[2] = current_joint_velocity.Actuator3 *M_PI/180; - traj_feedback_msg_.actual.velocities[3] = current_joint_velocity.Actuator4 *M_PI/180; - if (number_joint_>=6) + for(int actuator = 0; actuator < number_joint_; actuator++) { - traj_feedback_msg_.actual.velocities[4] = current_joint_velocity.Actuator5 *M_PI/180; - traj_feedback_msg_.actual.velocities[5] = current_joint_velocity.Actuator6 *M_PI/180; - if (number_joint_==7) - traj_feedback_msg_.actual.velocities[6] = current_joint_velocity.Actuator7 *M_PI/180; + traj_feedback_msg_.desired.positions[actuator] = current_joint_command.Actuators[actuator] *M_PI/180; + traj_feedback_msg_.actual.positions[actuator] = current_joint_angles[actuator] *M_PI/180; + traj_feedback_msg_.actual.velocities[actuator] = current_joint_velocity[actuator] *M_PI/180; } for (size_t j = 0; j Date: Thu, 17 Nov 2022 13:42:03 +0100 Subject: [PATCH 4/7] cleanup and execution fix in joint traj controller --- .../kinova_joint_trajectory_controller.h | 56 ++++++------------- .../kinova_joint_trajectory_controller.cpp | 48 ++++++++++++---- 2 files changed, 56 insertions(+), 48 deletions(-) diff --git a/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h b/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h index 0953c4132..207b65e31 100644 --- a/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h +++ b/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h @@ -1,7 +1,6 @@ #ifndef KINOVA_JOINT_TRAJECTORY_CONTROLLER_H #define KINOVA_JOINT_TRAJECTORY_CONTROLLER_H - #include #include #include @@ -26,46 +25,33 @@ class JointTrajectoryController private: ros::NodeHandle nh_; - ros::Subscriber sub_command_; - ros::Publisher pub_joint_feedback_; - ros::Publisher pub_joint_velocity_; + ros::Subscriber sub_command_; // command subscriber listening to 'trajectory_controller/command' + ros::Publisher pub_joint_feedback_; // feedback publisher sending to 'trajectory_controller/state' + ros::Publisher pub_joint_velocity_; // velocity publisher sending to 'in/joint_velocity' - ros::Time previous_pub_; - ros::Time time_pub_joint_vel_; + ros::Time time_pub_joint_vel_; // time of timer thread start to publish joint velocity command - ros::Timer timer_pub_joint_vel_; + ros::Timer timer_pub_joint_vel_; // timer to publish joint velocities boost::mutex terminate_thread_mutex_; boost::thread* thread_update_state_; bool terminate_thread_; - std_msgs::Duration time_from_start_; - sensor_msgs::JointState current_joint_state_; - - KinovaComm kinova_comm_; - TrajectoryPoint kinova_traj_point_; + KinovaComm kinova_comm_; // Communication pipeline for the arm -// trajectory_msgs::JointTrajectory joint_traj_; -// trajectory_msgs::JointTrajectoryPoint joint_traj_point_; - std::string traj_frame_id_; - std::vector traj_command_points_; - control_msgs::FollowJointTrajectoryFeedback traj_feedback_msg_; + std::string traj_frame_id_; // origin of trajectory transforms + std::vector traj_command_points_; // trajectory points from most recent trajectory + control_msgs::FollowJointTrajectoryFeedback traj_feedback_msg_; // trajectory feedback message // stores the command to send to robot, in Kinova type (KinovaAngles) - std::vector kinova_angle_command_; - - uint number_joint_; - int traj_command_points_index_; - std::vector joint_names_; - std::string prefix_; - - struct Segment - { - double start_time; - double duration; - std::vector positions; - std::vector velocities; - }; + std::vector kinova_angle_command_; // intermediate command storage + uint number_joint_; // number of joints of the robot + int traj_command_points_index_; // current index in traj_command_points_, defined by time + std::vector joint_names_; // names of the joints + std::string prefix_; // robot name prefix + const static int num_possible_joints = 7; // number of possible joints supported by the system + float current_velocity_command[num_possible_joints]; // storage array to keep calculated velocity commands + double remaining_motion_time[num_possible_joints]; // time of motion remaining for each joint during the last command // call back function when receive a trajectory command void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &traj_msg); @@ -73,17 +59,11 @@ class JointTrajectoryController // reflash the robot state and publish the joint state: either by timer or thread void update_state(); // by thread + // regularly publish the velocity commands of the trajectory void pub_joint_vel(const ros::TimerEvent&); // by timer - int test; }; - - - - - } - #endif // KINOVA_JOINT_TRAJECTORY_CONTROLLER_H diff --git a/kinova_driver/src/kinova_joint_trajectory_controller.cpp b/kinova_driver/src/kinova_joint_trajectory_controller.cpp index 0fc8502f6..4ada1f8cb 100644 --- a/kinova_driver/src/kinova_joint_trajectory_controller.cpp +++ b/kinova_driver/src/kinova_joint_trajectory_controller.cpp @@ -42,6 +42,7 @@ JointTrajectoryController::JointTrajectoryController(kinova::KinovaComm &kinova_ // counter in the timer to publish joint velocity command: pub_joint_vel() traj_command_points_index_ = 0; + for(int i=0; ipoints; ROS_INFO_STREAM_NAMED("Trajectory controller", "Receive trajectory with points number: " << traj_command_points_.size()); + if(traj_command_points_.empty()) return; // abort if // Map the index in joint_names and the msg std::vector lookup(number_joint_, -1); @@ -161,19 +163,45 @@ void JointTrajectoryController::pub_joint_vel(const ros::TimerEvent&) if (traj_command_points_index_ < kinova_angle_command_.size() && ros::ok()) { - joint_velocity_msg.joint1 = kinova_angle_command_[traj_command_points_index_][0]; - joint_velocity_msg.joint2 = kinova_angle_command_[traj_command_points_index_][1]; - joint_velocity_msg.joint3 = kinova_angle_command_[traj_command_points_index_][2]; - joint_velocity_msg.joint4 = kinova_angle_command_[traj_command_points_index_][3]; - joint_velocity_msg.joint5 = kinova_angle_command_[traj_command_points_index_][4]; - joint_velocity_msg.joint6 = kinova_angle_command_[traj_command_points_index_][5]; - joint_velocity_msg.joint7 = kinova_angle_command_[traj_command_points_index_][6]; + const ros::Duration current_time_from_start = ros::Time::now() - time_pub_joint_vel_; + // check for remaining motion time if in last command + if(traj_command_points_index_ == kinova_angle_command_.size()-1) + { + const double current_time = current_time_from_start.toSec(); + for(int i=0; i remaining_motion_time[i]) + current_velocity_command[i] = 0; + } + } + + joint_velocity_msg.joint1 = current_velocity_command[0]; + joint_velocity_msg.joint2 = current_velocity_command[1]; + joint_velocity_msg.joint3 = current_velocity_command[2]; + joint_velocity_msg.joint4 = current_velocity_command[3]; + joint_velocity_msg.joint5 = current_velocity_command[4]; + joint_velocity_msg.joint6 = current_velocity_command[5]; + joint_velocity_msg.joint7 = current_velocity_command[6]; pub_joint_velocity_.publish(joint_velocity_msg); - if( (ros::Time::now() - time_pub_joint_vel_) >= traj_command_points_[traj_command_points_index_].time_from_start) + if( current_time_from_start >= traj_command_points_[traj_command_points_index_].time_from_start) { ROS_INFO_STREAM("Moved to point " << traj_command_points_index_++); + for(int i=0; i Date: Fri, 26 May 2023 11:17:24 +0200 Subject: [PATCH 5/7] Added Services to change cartesian / angular cntrl --- .../include/kinova_driver/kinova_arm.h | 5 ++++ kinova_driver/src/kinova_arm.cpp | 26 +++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/kinova_driver/include/kinova_driver/kinova_arm.h b/kinova_driver/include/kinova_driver/kinova_arm.h index c0e194055..2854a128b 100644 --- a/kinova_driver/include/kinova_driver/kinova_arm.h +++ b/kinova_driver/include/kinova_driver/kinova_arm.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -78,6 +79,8 @@ class KinovaArm // Service callbacks ----------------------------------------------------------- bool stopServiceCallback(kinova_msgs::Stop::Request &req, kinova_msgs::Stop::Response &res); bool startServiceCallback(kinova_msgs::Start::Request &req, kinova_msgs::Start::Response &res); + bool cartesian_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + bool angular_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); bool homeArmServiceCallback(kinova_msgs::HomeArm::Request &req, kinova_msgs::HomeArm::Response &res); bool ActivateNullSpaceModeCallback(kinova_msgs::SetNullSpaceModeState::Request &req, kinova_msgs::SetNullSpaceModeState::Response &res); @@ -141,6 +144,8 @@ class KinovaArm ros::ServiceServer stop_service_; ros::ServiceServer start_service_; + ros::ServiceServer cartesian_control_service_; + ros::ServiceServer angular_control_service_; ros::ServiceServer homing_service_; ros::ServiceServer start_null_space_service_; ros::ServiceServer add_trajectory_; diff --git a/kinova_driver/src/kinova_arm.cpp b/kinova_driver/src/kinova_arm.cpp index 30fc89c34..710e53d41 100644 --- a/kinova_driver/src/kinova_arm.cpp +++ b/kinova_driver/src/kinova_arm.cpp @@ -165,6 +165,8 @@ KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const s /* Set up Services */ stop_service_ = node_handle_.advertiseService("in/stop", &KinovaArm::stopServiceCallback, this); start_service_ = node_handle_.advertiseService("in/start", &KinovaArm::startServiceCallback, this); + cartesian_control_service_ = node_handle_.advertiseService("in/cartesian_control", &KinovaArm::angular_controlServiceCallback, this); + angular_control_service_ = node_handle_.advertiseService("in/angular_control", &KinovaArm::angular_controlServiceCallback, this); homing_service_ = node_handle_.advertiseService("in/home_arm", &KinovaArm::homeArmServiceCallback, this); add_trajectory_ = node_handle_.advertiseService("in/add_pose_to_Cartesian_trajectory", &KinovaArm::addCartesianPoseToTrajectory, this); @@ -370,6 +372,30 @@ bool KinovaArm::stopServiceCallback(kinova_msgs::Stop::Request &req, kinova_msgs return true; } +/* + * Handler for service to change to cartesian control mode. + */ +bool KinovaArm::cartesian_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) +{ + kinova_comm_.setCartesianControl(); + res.message = "Cartesian Control activated"; + res.success = true; + ROS_DEBUG("Cartesian Control requested"); + return true; +} +/* + * Handler for service to change to angular control mode. + */ +bool KinovaArm::angular_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) +{ + kinova_comm_.setAngularControl(); + res.message = "Angular Control activated"; + res.success = true; + ROS_DEBUG("Angular Control requested"); + return true; +} + + /*! * \brief Handler for "start" service. From 5547a5a812d6bcf1dc131cb4374fc526daad07d8 Mon Sep 17 00:00:00 2001 From: f371xx Date: Tue, 30 May 2023 16:50:28 +0200 Subject: [PATCH 6/7] Implemented Joystick publishing --- .../include/kinova_driver/kinova_arm.h | 3 ++ .../include/kinova_driver/kinova_comm.h | 1 + kinova_driver/src/kinova_arm.cpp | 31 +++++++++++++++++++ kinova_driver/src/kinova_comm.cpp | 15 +++++++++ 4 files changed, 50 insertions(+) diff --git a/kinova_driver/include/kinova_driver/kinova_arm.h b/kinova_driver/include/kinova_driver/kinova_arm.h index 2854a128b..f8bb703b2 100644 --- a/kinova_driver/include/kinova_driver/kinova_arm.h +++ b/kinova_driver/include/kinova_driver/kinova_arm.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -119,6 +120,7 @@ class KinovaArm void publishToolPosition(void); void publishToolWrench(void); void publishFingerPosition(void); + void publishJoystickCommand(void); tf::TransformListener tf_listener_; ros::NodeHandle node_handle_; @@ -138,6 +140,7 @@ class KinovaArm ros::Publisher tool_wrench_publisher_; ros::Publisher finger_position_publisher_; ros::Publisher joint_state_publisher_; + ros::Publisher joystick_command_publisher_; ros::Publisher joint_command_publisher_; ros::Publisher cartesian_command_publisher_; diff --git a/kinova_driver/include/kinova_driver/kinova_comm.h b/kinova_driver/include/kinova_driver/kinova_comm.h index 8bedde24f..f95f2ffd9 100644 --- a/kinova_driver/include/kinova_driver/kinova_comm.h +++ b/kinova_driver/include/kinova_driver/kinova_comm.h @@ -88,6 +88,7 @@ class KinovaComm void initFingers(void); void setEndEffectorOffset(unsigned int status, float x, float y, float z); void getEndEffectorOffset(unsigned int &status, float &x, float &y, float &z); + void getJoystickCommand(JoystickCommand &joystick_command); // %EndTag(general functions) diff --git a/kinova_driver/src/kinova_arm.cpp b/kinova_driver/src/kinova_arm.cpp index 710e53d41..d03a619d4 100644 --- a/kinova_driver/src/kinova_arm.cpp +++ b/kinova_driver/src/kinova_arm.cpp @@ -204,6 +204,8 @@ KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const s ("out/tool_wrench", 2); finger_position_publisher_ = node_handle_.advertise ("out/finger_position", 2); + joystick_command_publisher_ = node_handle_.advertise + ("out/joystick_command", 2); // Publish last command for relative motion (read current position cause arm drop) joint_command_publisher_ = node_handle_.advertise("out/joint_command", 2); @@ -782,12 +784,41 @@ void KinovaArm::publishFingerPosition(void) finger_position_publisher_.publish(fingers.constructFingersMsg()); } +/** + * \brief Publishes the current command of the joystick + */ +void KinovaArm::publishJoystickCommand(void) +{ + JoystickCommand command; + kinova_comm_.getJoystickCommand(command); + + sensor_msgs::Joy msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = "kinova_joystick"; + msg.axes = {command.InclineLeftRight, + command.InclineForwardBackward, + command.Rotate, + command.MoveLeftRight, + command.MoveForwardBackward, + command.PushPull + }; + // The ButtonValue array seems to be ill-formed: + // Tests show it consists of 1-byte (char) sized values and is actually offset by 4 bytes + const int data_offset_bytes = 4; + const char* actual_button_array = (char*) command.ButtonValue + data_offset_bytes; + for(int i=0; i Date: Mon, 5 Jun 2023 13:57:06 +0200 Subject: [PATCH 7/7] Fixed Bug in the cartesian_control service --- kinova_driver/src/kinova_arm.cpp | 5 ++--- kinova_driver/src/kinova_comm.cpp | 1 - 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/kinova_driver/src/kinova_arm.cpp b/kinova_driver/src/kinova_arm.cpp index d03a619d4..bc9d85ab3 100644 --- a/kinova_driver/src/kinova_arm.cpp +++ b/kinova_driver/src/kinova_arm.cpp @@ -165,7 +165,7 @@ KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const s /* Set up Services */ stop_service_ = node_handle_.advertiseService("in/stop", &KinovaArm::stopServiceCallback, this); start_service_ = node_handle_.advertiseService("in/start", &KinovaArm::startServiceCallback, this); - cartesian_control_service_ = node_handle_.advertiseService("in/cartesian_control", &KinovaArm::angular_controlServiceCallback, this); + cartesian_control_service_ = node_handle_.advertiseService("in/cartesian_control", &KinovaArm::cartesian_controlServiceCallback, this); angular_control_service_ = node_handle_.advertiseService("in/angular_control", &KinovaArm::angular_controlServiceCallback, this); homing_service_ = node_handle_.advertiseService("in/home_arm", &KinovaArm::homeArmServiceCallback, this); add_trajectory_ = node_handle_.advertiseService("in/add_pose_to_Cartesian_trajectory", @@ -385,6 +385,7 @@ bool KinovaArm::cartesian_controlServiceCallback(std_srvs::Trigger::Request &req ROS_DEBUG("Cartesian Control requested"); return true; } + /* * Handler for service to change to angular control mode. */ @@ -397,8 +398,6 @@ bool KinovaArm::angular_controlServiceCallback(std_srvs::Trigger::Request &req, return true; } - - /*! * \brief Handler for "start" service. * diff --git a/kinova_driver/src/kinova_comm.cpp b/kinova_driver/src/kinova_comm.cpp index 40fed450e..b34f00a02 100644 --- a/kinova_driver/src/kinova_comm.cpp +++ b/kinova_driver/src/kinova_comm.cpp @@ -973,7 +973,6 @@ void KinovaComm::setCartesianControl() return; } int result = kinova_api_.setCartesianControl(); - ROS_WARN("%d", result); if (result != NO_ERROR_KINOVA) { throw KinovaCommException("Could not set Cartesian control", result);