Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[ros2] Port apply/clear wrench and effort services #941

Merged
merged 3 commits into from
Aug 14, 2019
Merged

[ros2] Port apply/clear wrench and effort services #941

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6d8327c
[ros2] Port apply/clear wrench and effort services
shiveshkhaitan Jun 23, 2019
df9092a
Change gazebo_ros_effort to gazebo_ros_force_system
shiveshkhaitan Aug 4, 2019
dac4f8c
Minor fixes
shiveshkhaitan Aug 8, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 0 additions & 62 deletions .ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,11 +45,6 @@
// Services
#include "std_srvs/Empty.h"

#include "gazebo_msgs/JointRequest.h"
#include "gazebo_msgs/BodyRequest.h"

#include "gazebo_msgs/ApplyBodyWrench.h"

#include "gazebo_msgs/SetPhysicsProperties.h"
#include "gazebo_msgs/GetPhysicsProperties.h"

Expand All @@ -60,7 +55,6 @@
#include "gazebo_msgs/GetModelProperties.h"

#include "gazebo_msgs/GetJointProperties.h"
#include "gazebo_msgs/ApplyJointEffort.h"

#include "gazebo_msgs/GetLinkProperties.h"
#include "gazebo_msgs/SetLinkProperties.h"
Expand All @@ -73,7 +67,6 @@
#include "gazebo_msgs/LinkStates.h"

#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Wrench.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Twist.h"

Expand Down Expand Up @@ -162,31 +155,11 @@ class GazeboRosApiPlugin : public SystemPlugin
/// \brief
bool setJointProperties(gazebo_msgs::SetJointProperties::Request &req,gazebo_msgs::SetJointProperties::Response &res);

/// \brief
bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,gazebo_msgs::ApplyJointEffort::Response &res);

/// \brief
bool clearJointForces(gazebo_msgs::JointRequest::Request &req,gazebo_msgs::JointRequest::Response &res);
bool clearJointForces(std::string joint_name);

/// \brief
bool clearBodyWrenches(gazebo_msgs::BodyRequest::Request &req,gazebo_msgs::BodyRequest::Response &res);
bool clearBodyWrenches(std::string body_name);

/// \brief
bool setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req,gazebo_msgs::SetModelConfiguration::Response &res);

/// \brief
bool applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req,gazebo_msgs::ApplyBodyWrench::Response &res);

private:

/// \brief
void wrenchBodySchedulerSlot();

/// \brief
void forceJointSchedulerSlot();

/// \brief
void publishSimTime(const boost::shared_ptr<gazebo::msgs::WorldStatistics const> &msg);
void publishSimTime();
Expand All @@ -197,13 +170,6 @@ class GazeboRosApiPlugin : public SystemPlugin
/// \brief
void publishModelStates();

/// \brief helper function for applyBodyWrench
/// shift wrench from reference frame to target frame
void transformWrench(ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque,
const ignition::math::Vector3d &reference_force,
const ignition::math::Vector3d &reference_torque,
const ignition::math::Pose3d &target_to_reference );

/// \brief Used for the dynamic reconfigure callback function template
void physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level);

Expand Down Expand Up @@ -231,8 +197,6 @@ class GazeboRosApiPlugin : public SystemPlugin
boost::shared_ptr<boost::thread> gazebo_callback_queue_thread_;

gazebo::physics::WorldPtr world_;
gazebo::event::ConnectionPtr wrench_update_event_;
gazebo::event::ConnectionPtr force_update_event_;
gazebo::event::ConnectionPtr time_update_event_;
gazebo::event::ConnectionPtr pub_link_states_event_;
gazebo::event::ConnectionPtr pub_model_states_event_;
Expand All @@ -247,12 +211,8 @@ class GazeboRosApiPlugin : public SystemPlugin
ros::ServiceServer set_link_properties_service_;
ros::ServiceServer set_physics_properties_service_;
ros::ServiceServer get_physics_properties_service_;
ros::ServiceServer apply_body_wrench_service_;
ros::ServiceServer set_joint_properties_service_;
ros::ServiceServer apply_joint_effort_service_;
ros::ServiceServer set_model_configuration_service_;
ros::ServiceServer clear_joint_forces_service_;
ros::ServiceServer clear_body_wrenches_service_;
ros::Publisher pub_link_states_;
ros::Publisher pub_model_states_;
int pub_link_states_connection_count_;
Expand All @@ -278,28 +238,6 @@ class GazeboRosApiPlugin : public SystemPlugin

bool world_created_;

class WrenchBodyJob
{
public:
gazebo::physics::LinkPtr body;
ignition::math::Vector3d force;
ignition::math::Vector3d torque;
ros::Time start_time;
ros::Duration duration;
};

class ForceJointJob
{
public:
gazebo::physics::JointPtr joint;
double force; // should this be a array?
ros::Time start_time;
ros::Duration duration;
};

std::vector<GazeboRosApiPlugin::WrenchBodyJob*> wrench_body_jobs_;
std::vector<GazeboRosApiPlugin::ForceJointJob*> force_joint_jobs_;

/// \brief enable the communication of gazebo information using ROS service/topics
bool enable_ros_network_;
};
Expand Down
Loading