robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

add attacher

Browse source
This commit is contained in:
Ilya Uraev 2022-03-16 00:49:48 +04:00
parent 91f159e289
commit ddba530489
3 changed files with 76 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

5
rasmt_support/world/robossembler.world
View file

@ -19,12 +19,9 @@
</spot>
</light>
<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>
</plugin>
<plugin name="ros_link_attacher_plugin" filename="libgazebo_ros_link_attacher_plugin.so"/>
<model name='ground_plane'>
<static>1</static>
<link name='link'>

5
robossembler_servers/CMakeLists.txt
View file

@ -28,6 +28,7 @@ find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(robossembler_interfaces REQUIRED)
find_package(rmw REQUIRED)
find_package(gazebo_ros_link_attacher REQUIRED)
set(deps
rclcpp
@ -40,6 +41,7 @@ set(deps
tf2_ros
rclcpp_components
robossembler_interfaces
gazebo_ros_link_attacher
)
#
# 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)
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(
TARGETS
move_topose_action_server
move_to_joint_states_action_server
move_cartesian_path_action_server
gripper_cmd_action_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib

74
robossembler_servers/src/gripper_cmd.cpp
View file

@ -8,9 +8,11 @@
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/float64_multi_array.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;
double finger_state{0};
namespace robossembler_servers
{
class GripperCmd : public rclcpp::Node
@ -29,16 +31,24 @@ public:
std::bind(&GripperCmd::accepted_callback, this, _1));
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:
rclcpp_action::Server<GripperCmdAction>::SharedPtr action_server_;
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>;
//double finger_state{};
rclcpp_action::GoalResponse goal_callback(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const GripperCmdAction::Goal> goal)
@ -70,12 +80,22 @@ private:
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<GripperCmdAction::Feedback>();
auto result = std::make_shared<GripperCmdAction::Result>();
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
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;
RCLCPP_INFO(this->get_logger(), "Initialize commands");
commands.data.push_back(goal->value);
@ -89,6 +109,52 @@ private:
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