Skip to content

Commit

Permalink
Multiple items attach/detach (altho repeating one crashes)
Browse files Browse the repository at this point in the history
  • Loading branch information
@awesomebytes
awesomebytes committed Apr 21, 2016
1 parent ef85182 commit 75a2ed4
Show file tree
Hide file tree
Showing 3 changed files with 202 additions and 14 deletions.
3 changes: 2 additions & 1 deletion include/gazebo_ros_link_attacher.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,8 @@ namespace gazebo
void detach_callback(const std_msgs::String msg);

bool attached;
physics::JointPtr fixedJoint;
physics::JointPtr fixedJoint[50];
int fixed_joint_counter;

physics::LinkPtr link1;
physics::LinkPtr link2;
Expand Down
183 changes: 183 additions & 0 deletions scripts/demo_multiple.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,183 @@
#!/usr/bin/env python

import rospy
from gazebo_ros_link_attacher.msg import Attach
from gazebo_msgs.srv import SpawnModel, SpawnModelRequest, SpawnModelResponse
from copy import deepcopy
from tf.transformations import quaternion_from_euler

sdf_cube = """<?xml version="1.0" ?>
<sdf version="1.4">
<model name="MODELNAME">
<static>0</static>
<link name="link">
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.01</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.01</iyy>
<iyz>0.0</iyz>
<izz>0.01</izz>
</inertia>
</inertial>
<collision name="stairs_collision0">
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>SIZEXYZ</size>
</box>
</geometry>
<surface>
<bounce />
<friction>
<ode>
<mu>1.0</mu>
<mu2>1.0</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>10000000.0</kp>
<kd>1.0</kd>
<min_depth>0.0</min_depth>
<max_vel>0.0</max_vel>
</ode>
</contact>
</surface>
</collision>
<visual name="stairs_visual0">
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>SIZEXYZ</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
</material>
</visual>
<velocity_decay>
<linear>0.000000</linear>
<angular>0.000000</angular>
</velocity_decay>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
</sdf>
"""


def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz):
"""Create a SpawnModelRequest with the parameters of the cube given.
modelname: name of the model for gazebo
px py pz: position of the cube (and it's collision cube)
rr rp ry: rotation (roll, pitch, yaw) of the model
sx sy sz: size of the cube"""
cube = deepcopy(sdf_cube)
# Replace size of model
size_str = str(round(sx, 3)) + " " + \
str(round(sy, 3)) + " " + str(round(sz, 3))
cube = cube.replace('SIZEXYZ', size_str)
# Replace modelname
cube = cube.replace('MODELNAME', str(modelname))

req = SpawnModelRequest()
req.model_name = modelname
req.model_xml = cube
req.initial_pose.position.x = px
req.initial_pose.position.y = py
req.initial_pose.position.z = pz

q = quaternion_from_euler(rr, rp, ry)
req.initial_pose.orientation.x = q[0]
req.initial_pose.orientation.y = q[1]
req.initial_pose.orientation.z = q[2]
req.initial_pose.orientation.w = q[3]

return req


if __name__ == '__main__':
rospy.init_node('demo_attach_links')
attach_pub = rospy.Publisher('/link_attacher_node/attach_models',
Attach, queue_size=1)
rospy.loginfo("Created publisher to /link_attacher_node/attach_models")
spawn_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
rospy.loginfo("Waiting for /gazebo/spawn_sdf_model service...")
spawn_srv.wait_for_service()
rospy.loginfo("Connected to service!")

# Spawn object 1
rospy.loginfo("Spawning cube1")
req1 = create_cube_request("cube1",
0.0, 0.0, 0.51, # position
0.0, 0.0, 0.0, # rotation
1.0, 1.0, 1.0) # size
spawn_srv.call(req1)
rospy.sleep(1.0)

# Spawn object 2
rospy.loginfo("Spawning cube2")
req2 = create_cube_request("cube2",
0.0, 1.1, 0.41, # position
0.0, 0.0, 0.0, # rotation
0.8, 0.8, 0.8) # size
spawn_srv.call(req2)
rospy.sleep(1.0)

# Spawn object 3
rospy.loginfo("Spawning cube3")
req3 = create_cube_request("cube3",
0.0, 2.1, 0.41, # position
0.0, 0.0, 0.0, # rotation
0.4, 0.4, 0.4) # size
spawn_srv.call(req3)
rospy.sleep(1.0)

# Link them
rospy.loginfo("Attaching cube1 and cube2")
amsg = Attach()
amsg.model_name_1 = "cube1"
amsg.link_name_1 = "link"
amsg.model_name_2 = "cube2"
amsg.link_name_2 = "link"

attach_pub.publish(amsg)
rospy.sleep(1.0)
# From the shell:
"""
rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'cube1'
link_name_1: 'link'
model_name_2: 'cube2'
link_name_2: 'link'"
"""
rospy.loginfo("Published into linking service!")


rospy.loginfo("Attaching cube2 and cube3")
amsg = Attach()
amsg.model_name_1 = "cube2"
amsg.link_name_1 = "link"
amsg.model_name_2 = "cube3"
amsg.link_name_2 = "link"

attach_pub.publish(amsg)
rospy.sleep(1.0)


rospy.loginfo("Attaching cube3 and cube1")
amsg = Attach()
amsg.model_name_1 = "cube3"
amsg.link_name_1 = "link"
amsg.model_name_2 = "cube1"
amsg.link_name_2 = "link"

attach_pub.publish(amsg)
rospy.sleep(2.0)
30 changes: 17 additions & 13 deletions src/gazebo_ros_link_attacher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,9 @@ namespace gazebo

this->world = _world;
this->physics = this->world->GetPhysicsEngine();
this->fixed_joint_counter = 0;
this->attach_by_name_subscriber_ = this->nh_.subscribe("attach_models", 1, &GazeboRosLinkAttacher::attach_callback, this);
this->detach_subscriber_ = this->nh_.subscribe("detach", 1, &GazeboRosLinkAttacher::detach_callback, this);
ROS_INFO("Link attacher node initialized");
}

Expand Down Expand Up @@ -84,17 +86,17 @@ namespace gazebo

ROS_INFO_STREAM("Creating revolute joint on model: '" << model1 << "'");
this->model = m1; // Store the model we created the joint, just in case
this->fixedJoint = this->physics->CreateJoint("revolute", m1);
this->fixedJoint[this->fixed_joint_counter] = this->physics->CreateJoint("revolute", m1);

this->link1 = l1; // Store the links too
this->link2 = l2;
ROS_INFO_STREAM("Attach");
this->fixedJoint->Attach(l1, l2);
this->fixedJoint[this->fixed_joint_counter]->Attach(l1, l2);
ROS_INFO_STREAM("Loading links");
this->fixedJoint->Load(l1,
this->fixedJoint[this->fixed_joint_counter]->Load(l1,
l2, math::Pose());
ROS_INFO_STREAM("SetModel");
this->fixedJoint->SetModel(m2);
this->fixedJoint[this->fixed_joint_counter]->SetModel(m2);
/*
* If SetModel is not done we get:
* ***** Internal Program Error - assertion (this->GetParentModel() != __null)
Expand All @@ -109,26 +111,28 @@ namespace gazebo
/tmp/buildd/gazebo2-2.2.3/gazebo/physics/ode/ODELink.cc(183): Inertial pointer is NULL
*/

ROS_INFO_STREAM("SetAxis");
this->fixedJoint->SetAxis(0, math::Vector3(0, 0, 1));
// ROS_INFO_STREAM("SetAxis");
// this->fixedJoint[this->fixed_joint_counter]->SetAxis(0, math::Vector3(0, 0, 1));
ROS_INFO_STREAM("SetHightstop");
this->fixedJoint->SetHighStop(0, 0);
this->fixedJoint[this->fixed_joint_counter]->SetHighStop(0, 0);
ROS_INFO_STREAM("SetLowStop");
this->fixedJoint->SetLowStop(0, 0);
ROS_INFO_STREAM("Giving a name");
this->fixedJoint->SetName("fixedjoint");
this->fixedJoint[this->fixed_joint_counter]->SetLowStop(0, 0);
// ROS_INFO_STREAM("Giving a name");
// this->fixedJoint[this->fixed_joint_counter]->SetName("fixedjoint");
ROS_INFO_STREAM("Init");
this->fixedJoint->Init();
this->fixedJoint[this->fixed_joint_counter]->Init();
ROS_INFO_STREAM("We are done");
this->attached = true;
this->fixed_joint_counter++;

return true;
}

bool GazeboRosLinkAttacher::detach()
{
this->fixedJoint->Detach();
this->fixedJoint->Fini();
this->fixedJoint[this->fixed_joint_counter-1]->Detach();
this->fixedJoint[this->fixed_joint_counter-1]->Fini();
this->fixedJoint[this->fixed_joint_counter-1]->Reset();
this->attached = false;
return true;
}
Expand Down

0 comments on commit 75a2ed4

Please sign in to comment.