41 #include <sensor_msgs/JointState.h> 42 #include <robotiq_3f_gripper_articulated_msgs/Robotiq3FGripperRobotInput.h> 43 #include <robotiq_3f_gripper_articulated_msgs/Robotiq3FGripperRobotOutput.h> 56 inline double joint1()
const;
57 inline double joint2()
const;
58 inline double joint3()
const;
98 joint_positions.resize(11, 0.0);
105 joint_positions.resize(11, 0.0);
106 prefix = gripper_prefix;
109 inline double scissorJoint()
const;
110 void callback(
const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput::ConstPtr &
msg);
116 std::vector<std::string> jointNames();
127 if(0 <= scissor && scissor <= 15)
return 11.0*
DEG_TO_RAD;
128 else if(15 < scissor && scissor <= 240)
return (11.0-22.0*((scissor-15.0)/225.0))*
DEG_TO_RAD;
135 void Robotiq3::callback(
const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput::ConstPtr &msg) {
136 finger_left =
Finger(msg->gPOA);
137 finger_right =
Finger(msg->gPOB);
138 finger_middle =
Finger(msg->gPOC);
142 joint_positions.at(0) = scissorJoint();
143 joint_positions.at(1) = finger_left.joint1();
144 joint_positions.at(2) = finger_left.joint2();
145 joint_positions.at(3) = finger_left.joint3();
146 joint_positions.at(4) = -joint_positions.at(0);
147 joint_positions.at(5) = finger_right.joint1();
148 joint_positions.at(6) = finger_right.joint2();
149 joint_positions.at(7) = finger_right.joint3();
150 joint_positions.at(8) = finger_middle.joint1();
151 joint_positions.at(9) = finger_middle.joint2();
152 joint_positions.at(10) = finger_middle.joint3();
161 std::vector<std::string> joint_names(11,
"");
162 joint_names.at(0).assign(prefix +
"palm_finger_1_joint");
163 joint_names.at(1).assign(prefix +
"finger_1_joint_1");
164 joint_names.at(2).assign(prefix +
"finger_1_joint_2");
165 joint_names.at(3).assign(prefix +
"finger_1_joint_3");
166 joint_names.at(4).assign(prefix +
"palm_finger_2_joint");
167 joint_names.at(5).assign(prefix +
"finger_2_joint_1");
168 joint_names.at(6).assign(prefix +
"finger_2_joint_2");
169 joint_names.at(7).assign(prefix +
"finger_2_joint_3");
170 joint_names.at(8).assign(prefix +
"finger_middle_joint_1");
171 joint_names.at(9).assign(prefix +
"finger_middle_joint_2");
172 joint_names.at(10).assign(prefix +
"finger_middle_joint_3");
179 int main(
int argc,
char *argv[]) {
182 ros::init(argc, argv,
"robotiq_3f_gripper_joint_states");
188 std::string gripper_prefix;
189 pnh.
param<std::string>(
"prefix", gripper_prefix,
"");
196 joint_pub = nh.
advertise<sensor_msgs::JointState>(
"joint_states", 10);
203 sensor_msgs::JointState joint_msg;
206 joint_msg.name = robotiq.jointNames();
209 joint_msg.position = robotiq.joint_positions;
int main(int argc, char *argv[])
Finger()
Default constructor for creating Finger, position is set 0.
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void callback(const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput::ConstPtr &msg)
Callback function for "Robotiq3FGripperRobotInput" topic.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int position
Position of the Finger.
std::string prefix
Gripper prefix.
double joint3() const
joint_3 value for Finger
double joint2() const
joint_2 value for Finger
Finger(const Finger &f)
Create Finger, position is taken form.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
double scissorJoint() const
Joint value for so-called scissor joint.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int scissor
Scissor position.
Finger finger_middle
Robotiq FINGER C.
Finger(int pos)
Create Finger, position is.
Finger finger_right
Robotiq FINGER B.
ROSCPP_DECL void spinOnce()
std::vector< double > joint_positions
Joint values.
Robotiq3(std::string gripper_prefix)
double joint1() const
joint_1 value for Finger
std::vector< std::string > jointNames()
Joint names.
Finger finger_left
Robotiq FINGER A.