mia_hand_driver
rel 1.0.0
|
Go to the documentation of this file.
14 ROS_INFO(
"Please specify Mia Hand port number: ");
21 std::string info_msg =
"/dev/ttyUSB successfully opened.";
22 info_msg.insert(11, std::to_string(port_num));
24 ROS_INFO(
"%s\n", info_msg.c_str());
43 ROS_ERROR(
"Could not open specified serial port.");
49 mia_hand_msgs::FingersData msg;
69 mia_hand_msgs::FingersStrainGauges msg_sg;
87 ROS_INFO(
"Mia Hand connected.");
92 ROS_INFO(
"Mia Hand disconnected.");
508 mia_hand_msgs::ConnectSerial::Request& req,
509 mia_hand_msgs::ConnectSerial::Response& resp)
516 resp.message =
"/dev/ttyUSB" + std::to_string(req.port)
517 +
" succesfully opened.";
521 resp.success =
false;
522 resp.message =
"Could not open /dev/ttyUSB" + std::to_string(req.port)
530 std_srvs::Trigger::Response& resp)
537 resp.message =
"Mia Hand serial port closed.";
541 resp.success =
false;
542 resp.message =
"Could not close Mia Hand serial port.";
549 std_srvs::SetBool::Response& resp)
557 std_srvs::SetBool::Response& resp)
565 std_srvs::SetBool::Response& resp)
573 std_srvs::SetBool::Response& resp)
581 std_srvs::Empty::Response& resp)
589 std_srvs::Empty::Response& resp)
597 std_srvs::Empty::Response& resp)
605 std_srvs::Empty::Response& resp)
613 std_srvs::Empty::Response& resp)
621 std_srvs::Empty::Response& resp)
629 std_srvs::Empty::Response& resp)
637 std_srvs::Empty::Response& resp)
645 std_srvs::Empty::Response& resp)
653 std_srvs::Empty::Response& resp)
ros::Subscriber thu_cyl_grasp_ref_
Subscriber to receive the parametrs for cylindrical grasp for the thumb flexion motor.
void getFingerSgRaw(uint8_t fin_id, int16_t &nor_for, int16_t &tan_for)
Get the current output of the force sensor of a specific finger of the Mia hand.
bool switchPosStreamCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
Callback of the service switch_pos_stream_.
void switchSpeStream(bool b_on_off)
Manage the streaming of the motors velocity data.
ros::Subscriber ind_mot_trgt_spe_
Subscriber to receive the target velocity of the index flexion motor.
void indFinTrgtForCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber ind_fin_trgt_for_.
bool openTriGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service open_tri_grasp_.
ros::Subscriber lat_grasp_percent_
Subscriber to receive the closure percentage for lateral grasp.
void mrlFinTrgtForCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber mrl_fin_trgt_for_.
bool was_connected_
True if the Mia hand was connected, False otherwise.
void mrlMotTrgtSpeCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber mrl_mot_trgt_spe_.
void thuMotTrgtPosCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber thu_mot_trgt_pos_.
ROSDriver(ros::NodeHandle &nh, ros::NodeHandle &nh_priv)
Class constructor.
void initServices()
Initialize services.
void checkConnectionTmrCallback(const ros::WallTimerEvent &event)
Callback of the check_connection_tmr_ timer.
ros::Subscriber ind_fin_trgt_for_
Subscriber to receive the target force of the index flexion motor.
void indLatGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber ind_lat_grasp_ref_.
ros::Subscriber pin_grasp_percent_
Subscriber to receive the closure percentage for pinch grasp.
void pinGraspPercentCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber pin_grasp_percent_.
bool closeSphGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service close_sph_grasp_.
void openGrasp(char grasp_id)
Go to rest pose of a grasp type Move all the Mia hand fingers to the rest pose of a specific grasp ty...
bool is_connected_
True if the Mia hand is connected, False otherwise.
bool openSphGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service open_sph_grasp_.
ros::Subscriber thu_fin_trgt_for_
Subscriber to receive the target force of the tumb flexion motor.
ros::Subscriber thu_pin_grasp_ref_
Subscriber to receive the parametrs for pinch grasp for the thumb flexion motor.
void setThuGraspRef(char grasp_id, int16_t rest, int16_t pos, int16_t delay)
Set grasp parameter for thumb flexion motor.
void mrlLatGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber #mrl_lat_grasp_ref.
ros::Subscriber thu_sph_grasp_ref_
Subscriber to receive the parametrs for spherical grasp for the thumb flexion motor.
ros::Subscriber tri_grasp_percent_
Subscriber to receive the closure percentage for tridigital grasp.
void setMotorSpe(uint8_t fin_id, int16_t mot_spe)
Set the target velocity of a specific motor of the Mia hand.
ros::Subscriber mrl_sph_grasp_ref_
Subscriber to receive the parametrs for spherical grasp for the mrl flexion motor.
ros::Subscriber ind_lat_grasp_ref_
Subscriber to receive the parametrs for lateral grasp for the index flexion motor.
ros::Subscriber ind_mot_trgt_pos_
Subscriber to receive the target pose of the index flexion motor.
void indMotTrgtPosCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber ind_mot_trgt_pos_.
void mrlPinGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber #mrl_pin_grasp_ref.
bool switchAnaStreamCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
Callback of the service switch_ana_stream_.
void indCylGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber ind_cyl_grasp_ref_.
bool openPinGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service open_pin_grasp_.
ros::Subscriber thu_tri_grasp_ref_
Subscriber to receive the parametrs for tridigital grasp for the thumb flexion motor.
ros::Subscriber sph_grasp_percent_
Subscriber to receive the closure percentage for sherical grasp.
ros::Subscriber mrl_mot_trgt_spe_
Subscriber to receive the target velocity of the mrl flexion motor.
void mrlCylGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber #mrl_cyl_grasp_ref.
void thuCylGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber thu_cyl_grasp_ref_.
void indMotTrgtSpeCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber ind_mot_trgt_spe_.
ros::Subscriber mrl_tri_grasp_ref_
Subscriber to receive the parametrs for tridigital grasp for the mrl flexion motor.
int16_t getMotorPos(uint8_t fin_id)
Get the current position of a specific motor of the Mia hand.
ros::ServiceServer open_lat_grasp_
Service to move the Mia hand motors to the rest position of the lateral grasp.
ros::Subscriber mrl_lat_grasp_ref_
Subscriber to receive the parametrs for lateral grasp for the mrl flexion motor.
bool disconnect()
Disconnect the Mia hand closing the serial port.
ros::Subscriber ind_cyl_grasp_ref_
Subscriber to receive the parametrs for cylindrical grasp for the index flexion motor.
ros::Publisher fin_for_info_
Publisher of the force data read by the sensors embedded in the Mia hand fingers.
void mrlSphGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber #mrl_sph_grasp_ref.
ros::Publisher mot_pos_info_
Publisher of the positions data of the Mia hand motors.
void thuLatGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber thu_lat_grasp_ref_.
void switchAnaStream(bool b_on_off)
Manage the streaming of analog input data (as the straing gauge force sensors).
ros::ServiceServer switch_spe_stream_
Service to manage a the streaming of motor velocity data sent by the Mia hand.
ros::NodeHandle & nh_
Reference to public node handle (for topics and services)
void triGraspPercentCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber tri_grasp_percent_.
int16_t getMotorSpe(uint8_t fin_id)
Get the current velocity of a specific motor of the Mia hand.
ros::Subscriber mrl_pin_grasp_ref_
Subscriber to receive the parametrs for pinch grasp for the mrl flexion motor.
void indTriGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber ind_tri_grasp_ref_.
void indSphGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber ind_sph_grasp_ref_.
ros::Subscriber mrl_fin_trgt_for_
Subscriber to receive the target force of the mrl flexion motor.
void cylGraspPercentCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber cyl_grasp_percent_.
CppDriver mia_
Cpp driver of the Mia hand.
ros::Subscriber thu_mot_trgt_spe_
Subscriber to receive the target velocity of the tumb flexion motor.
ros::Subscriber mrl_cyl_grasp_ref_
Subscriber to receive the parametrs for cylindrical grasp for the mrl flexion motor.
ros::Subscriber ind_pin_grasp_ref_
Subscriber to receive the parametrs for pinch grasp for the index flexion motor.
ros::Publisher mot_spe_info_
Publisher of the velocity data of the Mia hand motors.
ros::ServiceServer switch_cur_stream_
Service to manage a the streaming of the current data (i.e. force sensor outputs) sent by the Mia han...
void initSubscribersMrl()
Initialize subscribers of the index mrl motor info.
void switchPosStream(bool b_on_off)
Manage the streaming of the motors position data.
bool closeTriGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service close_tri_grasp_.
void thuTriGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber thu_sph_grasp_ref_.
bool isConnected()
Function returning the connection status of the Mia hand.
ros::Subscriber ind_sph_grasp_ref_
Subscriber to receive the parametrs for spherical grasp for the index flexion motor.
void thuPinGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber thu_pin_grasp_ref_.
ros::ServiceServer close_cyl_grasp_
Service to move the Mia hand motors to the closed position of the cylindrical grasp.
void setMrlGraspRef(char grasp_id, int16_t rest, int16_t pos, int16_t delay)
Set grasp parameter for MRL flexion motor.
ros::ServiceServer close_lat_grasp_
Service to move the Mia hand motors to the closed position of the lateral grasp.
ros::WallTimer check_connection_tmr_
Timer to handle the check of the Mia hand connection status.
void sphGraspPercentCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber sph_grasp_percent_.
void initSubscribersGrasp()
Initialize subscribers of the automatic grasp info.
int16_t getMotorCur(uint8_t fin_id)
Get the current currently absorbed by a specific motor of the Mia hand.
ros::Subscriber mrl_mot_trgt_pos_
Subscriber to receive the target pose of the mrl flexion motor.
void thuMotTrgtSpeCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber thu_mot_trgt_spe_.
void initSubscribersInd()
Initialize subscribers of the index flexion motor info.
ros::ServiceServer close_sph_grasp_
Service to move the Mia hand motors to the closed position of the spherical grasp.
ros::ServiceServer open_sph_grasp_
Service to move the Mia hand motors to the rest position of the spherical grasp.
ros::WallTimer publish_data_tmr_
Timer to handle the Mia hand data publishing.
void mrlMotTrgtPosCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber mrl_mot_trgt_pos_.
void latGraspPercentCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber lat_grasp_percent_.
bool disconnectCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &resp)
Callback of the service disconnect_.
bool openCylGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service open_cyl_grasp_.
void publishDataTmrCallback(const ros::WallTimerEvent &event)
Callback of the publish_data_tmr_ timer.
bool connectToPort(uint16_t port_num)
Open serial port used to plug Mia hand.
void closeGrasp(char grasp_id)
Go to closed pose of a grasp type Move all the Mia hand fingers to the closed pose of a specific gras...
void thuSphGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber thu_sph_grasp_ref_.
bool connectToPortCallback(mia_hand_msgs::ConnectSerial::Request &req, mia_hand_msgs::ConnectSerial::Response &resp)
Callback of the service connect_to_port_.
ros::ServiceServer disconnect_
Service to disconnect the Mia hand and close the serial port.
bool switchCurStreamCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
Callback of the service switch_cur_stream_.
void setMotorPos(uint8_t fin_id, int16_t mot_pos)
Set the target position of a specific motor of the Mia hand.
void mrlTriGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber #mrl_tri_grasp_ref.
ros::Subscriber cyl_grasp_percent_
Subscriber to receive the closure percentage for cylindical grasp.
void setIndGraspRef(char grasp_id, int16_t rest, int16_t pos, int16_t delay)
Set grasp parameter for Index flexion + thumb abduction motor.
ros::Subscriber thu_lat_grasp_ref_
Subscriber to receive the parametrs for lateral grasp for the thumb flexion motor.
ros::Subscriber ind_tri_grasp_ref_
Subscriber to receive the parametrs for tridigital grasp for the index flexion motor.
ros::ServiceServer switch_pos_stream_
Service to manage a the streaming of motor position data sent by the Mia hand.
ros::ServiceServer open_cyl_grasp_
Service to move the Mia hand motors to the rest position of the cylindrical grasp.
void initSubscribersThu()
Initialize subscribers of the thumb flexion motor info.
ros::ServiceServer open_tri_grasp_
Service to move the Mia hand motors to the rest position of the tridigital grasp.
void initPublishers()
Initialize publishers.
bool closeCylGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service close_cyl_grasp_.
bool closePinGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service close_pin_grasp_.
void switchCurStream(bool b_on_off)
Manage the streaming of the current absorbed by Mi hand motors.
bool closeLatGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service close_lat_grasp_.
ros::ServiceServer switch_ana_stream_
Service to manage a the streaming of the analog input data (i.e. force sensor outputs) sent by the Mi...
ros::ServiceServer close_pin_grasp_
Service to move the Mia hand motors to the closed position of the pinch grasp.
void indPinGraspRefCallback(const mia_hand_msgs::GraspRef::ConstPtr &msg)
Callback of the subscriber ind_pin_grasp_ref_.
ros::Publisher mot_cur_info_
Publisher of the current absorbed by the Mia hand motors.
void setFingerFor(uint8_t fin_id, int16_t fin_for)
Set the target force of a specific motor of the Mia hand.
bool openLatGraspCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
Callback of the service open_lat_grasp_.
ros::ServiceServer close_tri_grasp_
Service to move the Mia hand motors to the closed position of the tridigital grasp.
bool switchSpeStreamCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
Callback of the service switch_spe_stream_.
void thuFinTrgtForCallback(const std_msgs::Int16::ConstPtr &msg)
Callback of the subscriber thu_fin_trgt_for_.
ros::Subscriber thu_mot_trgt_pos_
Subscriber to receive the target pose of the tumb flexion motor.
ros::ServiceServer open_pin_grasp_
Service to move the Mia hand motors to the rest position of the pinch grasp.
ros::ServiceServer connect_to_port_
Service to open a serial port and connect the Mia hand.