add attacher
This commit is contained in:
parent
91f159e289
commit
ddba530489
3 changed files with 76 additions and 8 deletions
|
@ -19,12 +19,9 @@
|
||||||
</spot>
|
</spot>
|
||||||
</light>
|
</light>
|
||||||
<plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
|
<plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
|
||||||
<ros>
|
|
||||||
<!--namespace>/rasmt</namespace-->
|
|
||||||
<argument>model_states:=model_states_demo</argument>
|
|
||||||
</ros>
|
|
||||||
<update_rate>1.0</update_rate>
|
<update_rate>1.0</update_rate>
|
||||||
</plugin>
|
</plugin>
|
||||||
|
<plugin name="ros_link_attacher_plugin" filename="libgazebo_ros_link_attacher_plugin.so"/>
|
||||||
<model name='ground_plane'>
|
<model name='ground_plane'>
|
||||||
<static>1</static>
|
<static>1</static>
|
||||||
<link name='link'>
|
<link name='link'>
|
||||||
|
|
|
@ -28,6 +28,7 @@ find_package(tf2_ros REQUIRED)
|
||||||
find_package(geometry_msgs REQUIRED)
|
find_package(geometry_msgs REQUIRED)
|
||||||
find_package(robossembler_interfaces REQUIRED)
|
find_package(robossembler_interfaces REQUIRED)
|
||||||
find_package(rmw REQUIRED)
|
find_package(rmw REQUIRED)
|
||||||
|
find_package(gazebo_ros_link_attacher REQUIRED)
|
||||||
|
|
||||||
set(deps
|
set(deps
|
||||||
rclcpp
|
rclcpp
|
||||||
|
@ -40,6 +41,7 @@ set(deps
|
||||||
tf2_ros
|
tf2_ros
|
||||||
rclcpp_components
|
rclcpp_components
|
||||||
robossembler_interfaces
|
robossembler_interfaces
|
||||||
|
gazebo_ros_link_attacher
|
||||||
)
|
)
|
||||||
#
|
#
|
||||||
# include_directories(include)
|
# include_directories(include)
|
||||||
|
@ -67,11 +69,14 @@ ament_target_dependencies(move_to_joint_states_action_server ${deps})
|
||||||
add_executable(move_topose_action_server src/move_topose_action_server.cpp)
|
add_executable(move_topose_action_server src/move_topose_action_server.cpp)
|
||||||
ament_target_dependencies(move_topose_action_server ${deps})
|
ament_target_dependencies(move_topose_action_server ${deps})
|
||||||
|
|
||||||
|
add_executable(move_cartesian_path_action_server src/move_cartesian_path_action_server.cpp)
|
||||||
|
ament_target_dependencies(move_cartesian_path_action_server ${deps})
|
||||||
|
|
||||||
install(
|
install(
|
||||||
TARGETS
|
TARGETS
|
||||||
move_topose_action_server
|
move_topose_action_server
|
||||||
move_to_joint_states_action_server
|
move_to_joint_states_action_server
|
||||||
|
move_cartesian_path_action_server
|
||||||
gripper_cmd_action_server
|
gripper_cmd_action_server
|
||||||
ARCHIVE DESTINATION lib
|
ARCHIVE DESTINATION lib
|
||||||
LIBRARY DESTINATION lib
|
LIBRARY DESTINATION lib
|
||||||
|
|
|
@ -8,9 +8,11 @@
|
||||||
#include "rclcpp_components/register_node_macro.hpp"
|
#include "rclcpp_components/register_node_macro.hpp"
|
||||||
#include "std_msgs/msg/float64_multi_array.hpp"
|
#include "std_msgs/msg/float64_multi_array.hpp"
|
||||||
#include "robossembler_interfaces/action/gripper_command.hpp"
|
#include "robossembler_interfaces/action/gripper_command.hpp"
|
||||||
|
#include "gazebo_ros_link_attacher/srv/attach.hpp"
|
||||||
|
#include "sensor_msgs/msg/joint_state.hpp"
|
||||||
|
|
||||||
using GripperCmdAction = robossembler_interfaces::action::GripperCommand;
|
using GripperCmdAction = robossembler_interfaces::action::GripperCommand;
|
||||||
|
double finger_state{0};
|
||||||
namespace robossembler_servers
|
namespace robossembler_servers
|
||||||
{
|
{
|
||||||
class GripperCmd : public rclcpp::Node
|
class GripperCmd : public rclcpp::Node
|
||||||
|
@ -29,16 +31,24 @@ public:
|
||||||
std::bind(&GripperCmd::accepted_callback, this, _1));
|
std::bind(&GripperCmd::accepted_callback, this, _1));
|
||||||
|
|
||||||
this->gripper_cmd_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>
|
this->gripper_cmd_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>
|
||||||
("/rasmt_hand_controller/command", 10);
|
("/rasmt_hand_controller/commands", 10);
|
||||||
|
this->attach_srv_ = this->create_client<gazebo_ros_link_attacher::srv::Attach>("attach");
|
||||||
|
this->detach_srv_ = this->create_client<gazebo_ros_link_attacher::srv::Attach>("detach");
|
||||||
|
|
||||||
|
this->joint_states_ = this->create_subscription<sensor_msgs::msg::JointState>
|
||||||
|
("/joint_states", 10, std::bind(&robossembler_servers::GripperCmd::get_fingers_state, this, _1));
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
rclcpp_action::Server<GripperCmdAction>::SharedPtr action_server_;
|
rclcpp_action::Server<GripperCmdAction>::SharedPtr action_server_;
|
||||||
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_publisher_;
|
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_publisher_;
|
||||||
|
rclcpp::Client<gazebo_ros_link_attacher::srv::Attach>::SharedPtr attach_srv_;
|
||||||
|
rclcpp::Client<gazebo_ros_link_attacher::srv::Attach>::SharedPtr detach_srv_;
|
||||||
|
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_;
|
||||||
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCmdAction>;
|
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCmdAction>;
|
||||||
|
|
||||||
|
//double finger_state{};
|
||||||
|
|
||||||
rclcpp_action::GoalResponse goal_callback(
|
rclcpp_action::GoalResponse goal_callback(
|
||||||
const rclcpp_action::GoalUUID & uuid,
|
const rclcpp_action::GoalUUID & uuid,
|
||||||
std::shared_ptr<const GripperCmdAction::Goal> goal)
|
std::shared_ptr<const GripperCmdAction::Goal> goal)
|
||||||
|
@ -70,12 +80,22 @@ private:
|
||||||
const auto goal = goal_handle->get_goal();
|
const auto goal = goal_handle->get_goal();
|
||||||
auto feedback = std::make_shared<GripperCmdAction::Feedback>();
|
auto feedback = std::make_shared<GripperCmdAction::Feedback>();
|
||||||
auto result = std::make_shared<GripperCmdAction::Result>();
|
auto result = std::make_shared<GripperCmdAction::Result>();
|
||||||
|
|
||||||
if (goal_handle->is_canceling()) {
|
if (goal_handle->is_canceling()) {
|
||||||
goal_handle->canceled(result);
|
goal_handle->canceled(result);
|
||||||
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
|
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
// Compute finger change for detach or attach
|
||||||
|
auto finger_state_change = goal->value - finger_state;
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "Finger change [%.2f]", finger_state_change);
|
||||||
|
if(finger_state_change<0.0)
|
||||||
|
{
|
||||||
|
send_attach_goal("rasmt", "cube", "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
|
||||||
|
} else if (finger_state_change>0.0)
|
||||||
|
{
|
||||||
|
send_detach_goal("rasmt", "cube", "rasmt_Dock_Link", "link");
|
||||||
|
}
|
||||||
|
// Send commands to gripper
|
||||||
std_msgs::msg::Float64MultiArray commands;
|
std_msgs::msg::Float64MultiArray commands;
|
||||||
RCLCPP_INFO(this->get_logger(), "Initialize commands");
|
RCLCPP_INFO(this->get_logger(), "Initialize commands");
|
||||||
commands.data.push_back(goal->value);
|
commands.data.push_back(goal->value);
|
||||||
|
@ -89,6 +109,52 @@ private:
|
||||||
RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
|
RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void send_attach_goal(const std::string model1_name, std::string model2_name,
|
||||||
|
std::string model1_link_name, std::string model2_link_name)
|
||||||
|
{
|
||||||
|
auto request = std::make_shared<gazebo_ros_link_attacher::srv::Attach::Request>();
|
||||||
|
request->model_name_1 = model1_name;
|
||||||
|
request->model_name_2 = model2_name;
|
||||||
|
request->link_name_1 = model1_link_name;
|
||||||
|
request->link_name_2 = model2_link_name;
|
||||||
|
|
||||||
|
while (!attach_srv_->wait_for_service(std::chrono::seconds(1))) {
|
||||||
|
if (!rclcpp::ok()) {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "Interruped while waiting for the server.");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
RCLCPP_INFO(this->get_logger(), "Server not available, waiting again...");
|
||||||
|
}
|
||||||
|
|
||||||
|
auto result = attach_srv_->async_send_request(request);
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_detach_goal(const std::string model1_name, std::string model2_name,
|
||||||
|
std::string model1_link_name, std::string model2_link_name)
|
||||||
|
{
|
||||||
|
auto request = std::make_shared<gazebo_ros_link_attacher::srv::Attach::Request>();
|
||||||
|
request->model_name_1 = model1_name;
|
||||||
|
request->model_name_2 = model2_name;
|
||||||
|
request->link_name_1 = model1_link_name;
|
||||||
|
request->link_name_2 = model2_link_name;
|
||||||
|
|
||||||
|
while (!detach_srv_->wait_for_service(std::chrono::seconds(1))) {
|
||||||
|
if (!rclcpp::ok()) {
|
||||||
|
RCLCPP_ERROR(this->get_logger(), "Interruped while waiting for the server.");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
RCLCPP_INFO(this->get_logger(), "Server not available, waiting again...");
|
||||||
|
}
|
||||||
|
|
||||||
|
auto result = detach_srv_->async_send_request(request);
|
||||||
|
}
|
||||||
|
|
||||||
|
void get_fingers_state(const sensor_msgs::msg::JointState::SharedPtr msg) const
|
||||||
|
{
|
||||||
|
finger_state = msg->position.back();
|
||||||
|
//RCLCPP_INFO(this->get_logger(), "Current joint position [%.5f]", joint_state);
|
||||||
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
} // namespace robossembler_servers
|
} // namespace robossembler_servers
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue