[Jderobot-admin] jderobot-r890 - in trunk/src/components/gazeboserver: . models
ahcorde en jderobot.org
ahcorde en jderobot.org
Vie Mar 22 12:08:30 CET 2013
Author: ahcorde
Date: 2013-03-22 12:07:30 +0100 (Fri, 22 Mar 2013)
New Revision: 890
Removed:
trunk/src/components/gazeboserver/Media/
trunk/src/components/gazeboserver/models/pioneer2dx.model
trunk/src/components/gazeboserver/models/pioneer2dxJde.model
trunk/src/components/gazeboserver/models/pioneer2dxJde.world
trunk/src/components/gazeboserver/models/pioneer2dxJdeFinal.model
trunk/src/components/gazeboserver/pioneer2dxJde_antiguo.world
trunk/src/components/gazeboserver/sonyvid30/
Modified:
trunk/src/components/gazeboserver/camera_dump.cc
trunk/src/components/gazeboserver/encoders.cc
trunk/src/components/gazeboserver/encoders.h
trunk/src/components/gazeboserver/laser.cc
trunk/src/components/gazeboserver/motors.cc
trunk/src/components/gazeboserver/motors.h
trunk/src/components/gazeboserver/pioneer2dxJde.world
trunk/src/components/gazeboserver/pose3dencoders.cc
trunk/src/components/gazeboserver/pose3dencoders.h
Log:
[ahcorde] Actualizo Gazeboserver para que acepte multirobot.
Elimino los recursos de versiones anteriores
Modified: trunk/src/components/gazeboserver/camera_dump.cc
===================================================================
--- trunk/src/components/gazeboserver/camera_dump.cc 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/camera_dump.cc 2013-03-22 11:07:30 UTC (rev 890)
@@ -5,7 +5,7 @@
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
-//#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/highgui/highgui.hpp>
// ICE utils includes
#include <Ice/Ice.h>
@@ -55,12 +55,16 @@
if(count==0){
std::vector<std::string> tokens;
nameCamera = this->parentSensor->GetCamera()->GetName();
- // std::cout << "Camera name: " + nameCamera << std::endl;
boost::split(tokens, nameCamera, boost::is_any_of("::"));
boost::split(tokens, tokens[2], boost::is_any_of("("));
nameGlobal = tokens[0];
+
+ std::string nameParent = this->parentSensor->GetParentName();
+ std::vector<std::string> strs;
+ boost::split(strs, nameParent, boost::is_any_of("::"));
+
// El nombre del fichero de configuracion tiene que coincidir con el de la cámara en el .world y el .cfg
- nameCamera = std::string("--Ice.Config=" + tokens[0] + ".cfg");
+ nameCamera = std::string("--Ice.Config=" + strs[0] + "_" + tokens[0] + ".cfg");
if (count == 0){
pthread_t thr_gui;
Modified: trunk/src/components/gazeboserver/encoders.cc
===================================================================
--- trunk/src/components/gazeboserver/encoders.cc 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/encoders.cc 2013-03-22 11:07:30 UTC (rev 890)
@@ -1,18 +1,15 @@
#include "encoders.h"
namespace gazebo {
-
+
void *encodersICE(void* v);
- int32_t secs;
+
GZ_REGISTER_MODEL_PLUGIN(Encoders)
Encoders::Encoders() {
pthread_mutex_init(&mutex, NULL);
pthread_mutex_init(&mutexEncoders, NULL);
count = 0;
- common::Time time;
- time = time.GetWallTime();
- secs = time.sec;
std::cout << "constructor Encoders" << std::endl;
}
@@ -22,13 +19,14 @@
this->updateConnection = event::Events::ConnectWorldUpdateStart(
boost::bind(&Encoders::OnUpdate, this));
}
+
+ physics::ModelPtr Encoders::getModel()
+ {
+ return model;
+ }
void Encoders::OnUpdate() {
- gettimeofday(&a, NULL);
- totala = a.tv_sec * 1000000 + a.tv_usec;
- cycle = 50;
-
-
+
if (count == 0) {
count++;
std::string name = this->model->GetName();
@@ -53,33 +51,7 @@
robotEncoders.x = position.pos.x;
robotEncoders.y = position.pos.y;
robotEncoders.theta = degrees;
- //std::cout << "x: " << robotEncoders.x << std::endl;
- //std::cout << "y: " << robotEncoders.y << std::endl;
pthread_mutex_unlock(&mutex);
-
- common::Time time;
-
- //std::cout << time.GetWallTime() << std::endl;
-
- time = time.GetWallTime();
- //std::cout << time.sec - secs << std::endl;
- //std::cout << "Vel: " << robotEncoders.x/(time.sec - secs) << "m/s" << std::endl;
- if((time.sec - secs) % 10 == 0){
- //std::cout << "x: " << robotEncoders.x << std::endl;
- }
-
-
- gettimeofday(&b, NULL);
- totalb = b.tv_sec * 1000000 + b.tv_usec;
-
- diff = (totalb - totala) / 1000;
- diff = cycle - diff;
-
- if (diff < 10)
- diff = 10;
-
- sleep(diff/1000);
- //MSleep(diff*1000);
}
@@ -92,14 +64,29 @@
virtual ~EncodersI() {
};
+
+ virtual void setEncodersData(const jderobot::EncodersDataPtr& encodersData,
+ const Ice::Current&) {
+ math::Pose position = this->pose->getModel()->GetWorldPose();
+
+ position.Set(encodersData->robotx,
+ encodersData->roboty,
+ encodersData->robotcos,
+ 0,
+ 0,
+ encodersData->robottheta);
+ this->pose->getModel()->SetWorldPose(position);
+ //this->getModel();
+ }
+
virtual jderobot::EncodersDataPtr getEncodersData(const Ice::Current&) {
pthread_mutex_lock(&pose->mutex);
//std::cout << "theta: " << pose->robotEncoders.theta << std::endl;
- encodersData->robotx = pose->robotEncoders.x ;
- encodersData->roboty = pose->robotEncoders.y ;
+ encodersData->robotx = pose->robotEncoders.x * 1000;
+ encodersData->roboty = pose->robotEncoders.y * 1000;
encodersData->robottheta = pose->robotEncoders.theta;
encodersData->robotcos = cos(pose->robotEncoders.theta);
encodersData->robotsin = sin(pose->robotEncoders.theta);
Modified: trunk/src/components/gazeboserver/encoders.h
===================================================================
--- trunk/src/components/gazeboserver/encoders.h 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/encoders.h 2013-03-22 11:07:30 UTC (rev 890)
@@ -26,6 +26,7 @@
Encoders();
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
+ physics::ModelPtr getModel();
pthread_mutex_t mutex;
pthread_mutex_t mutexEncoders;
@@ -47,10 +48,8 @@
physics::ModelPtr model;
math::Pose position;
event::ConnectionPtr updateConnection;
- int cycle;
- long totalb, totala, diff;
- struct timeval a, b;
+
};
}
Modified: trunk/src/components/gazeboserver/laser.cc
===================================================================
--- trunk/src/components/gazeboserver/laser.cc 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/laser.cc 2013-03-22 11:07:30 UTC (rev 890)
@@ -7,6 +7,9 @@
#include "plugins/RayPlugin.hh"
+#include <boost/algorithm/string.hpp>
+
+
#include <iostream>
// ICE utils includes
@@ -21,6 +24,7 @@
{
class LaserDump : public RayPlugin
{
+
public: LaserDump() : RayPlugin()
{
std::cout << "LaserDump Constructor" <<std::endl;
@@ -41,8 +45,13 @@
{
if(count == 0){
count++;
- std::string name = this->parentSensor->GetName();
- nameLaser = std::string("--Ice.Config=" + name + ".cfg");
+ std::string name = this->parentSensor->GetParentName();
+ std::cout <<" laser: " << name << std::endl;
+
+ std::vector<std::string> strs;
+ boost::split(strs, name, boost::is_any_of("::"));
+
+ nameLaser = std::string("--Ice.Config=" + strs[0] + "_laser.cfg");
pthread_t thr_gui;
pthread_create(&thr_gui, NULL, &mainLaser, (void*)this);
}
@@ -82,7 +91,7 @@
laserData->distanceData.resize(sizeof(int)*laserData->numLaser);
//Update laser values
- for(int i = laserData->numLaser-1 ; i > 0; i--){
+ for(int i = 0 ; i < laserData->numLaser; i++){
laserData->distanceData[i] = laser->laserValues[i]*1000;
}
pthread_mutex_unlock (&laser->mutex);
Deleted: trunk/src/components/gazeboserver/models/pioneer2dx.model
===================================================================
--- trunk/src/components/gazeboserver/models/pioneer2dx.model 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/models/pioneer2dx.model 2013-03-22 11:07:30 UTC (rev 890)
@@ -1,268 +0,0 @@
-<gazebo version='1.0'>
- <model name="pioneer2dx" static="0">
- <link name="chassis">
- <origin pose="0.0 0.0 0.16 0 0 0"/>
-
- <inertial mass="5.0"/>
- <!--<origin pose="0.0 0 -0.07 0 0 0"/>
- <inertia ixx='0.01' ixy='0' ixz='0' iyy='0.01' iyz='0' izz='0.01'/>
- </inertial> -->
-
- <collision name="collision">
- <geometry>
- <box size="0.445 0.277 0.17"/>
- </geometry>
- </collision>
-
- <visual name="visual">
- <origin pose="0 0 0.04 0 0 0"/>
- <geometry>
- <mesh filename="pioneer2dx/chassis.dae"/>
- </geometry>
- </visual>
-
-
- <collision name="castor_collision">
- <origin pose="-0.200 0 -0.12 0 0 0"/>
- <geometry>
- <sphere radius="0.04"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="0" mu2="0" slip1="1.0" slip2="1.0"/>
- </friction>
- </surface>
- </collision>
-
- <visual name="castor_visual">
- <origin pose="-0.200 0 -0.12 0 0 0"/>
- <geometry>
- <sphere radius="0.04"/>
- </geometry>
- <material script="Gazebo/FlatBlack"/>
- </visual>
-
- </link>
-
- <link name="laser">
- <inertial mass="0.5">
- </inertial>
- <visual name="visual">
- <geometry>
- <mesh filename="hokuyo/hokuyo.dae"/>
- </geometry>
- </visual>
- <origin pose='0.16 0 0.3 0 0 -1.57'/>
- <sensor name='laser' type='ray' always_on='1' update_rate='30' visualize='true'>
- <ray>
- <scan>
- <horizontal samples='180' resolution='1' min_angle='3.14' max_angle='0'/>
- </scan>
- <range min='0.080000000000000002' max='8' resolution='0.01'/>
- </ray>
- <plugin name='laser' filename='liblaser.so'/>
- </sensor>
- </link>
-
- <link name="camera_column_body_left">
- <origin pose="0.12 0.1 0.31 1.57 3.15 0"/>
- <inertial mass="0.0000000000000000000000000000000000000000000000005">
- </inertial>
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_mid.dae" scale="0.0125 0.0275 0.0275"/>
- </geometry>
- <material script='Gazebo/Blue'/>
- </visual>
- </link>
- <link name="camera_top_body_left">
- <origin pose="0.1 0.1 0.31 1.57 3.15 0"/>
- <inertial mass="0.0000000000000000000000000000000000000000000000005">
- </inertial>
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_top.dae" scale="0.04 0.04 0.03"/>
- </geometry>
-
- <material script='Gazebo/Black'/>
- </visual>
-
- <sensor name='cam_sensor_left' type='camera' always_on='1' update_rate='20'>
- <origin pose="0 0 0 1.57 3.15 0"/>
- <camera>
- <horizontal_fov angle='1.57'/>
- <image width='640' height='480' format='R8G8B8'/>
- <clip near='0.5' far='5'/>
- </camera>
- <plugin name='camera_dump' filename='libcamera_dump.so'/>
- </sensor>
- </link>
-
- <link name="camera_column_body_right">
- <origin pose="0.12 -0.1 0.31 1.57 3.15 0"/>
- <inertial mass="0.0000000000000000000000000000000000000000000000005">
- </inertial>
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_mid.dae" scale="0.0125 0.0275 0.0275"/>
- </geometry>
- <material script='Gazebo/Blue'/>
- </visual>
- </link>
- <link name="camera_top_body_right">
- <origin pose="0.1 -0.1 0.31 1.57 3.15 0"/>
- <inertial mass="0.0000000000000000000000000000000000000000000000005">
- </inertial>
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_top.dae" scale="0.04 0.04 0.03"/>
- </geometry>
-
- <material script='Gazebo/Black'/>
- </visual>
- <sensor name='cam_sensor_right' type='camera' always_on='1' update_rate='20'>
- <origin pose="0 0 0 1.57 3.15 0"/>
- <camera>
- <horizontal_fov angle='1.57'/>
- <image width='640' height='480' format='R8G8B8'/>
- <clip near='0.5' far='5'/>
- </camera>
- <plugin name='camera_dump' filename='libcamera_dump.so'/>
- </sensor>
- </link>
-
-
- <link name="right_wheel">
- <origin pose="0.1 -.17 0.11 0 1.5707 1.5707"/>
-
- <inertial mass="0.5">
- <inertia ixx='0.001' ixy='0.0' ixz='0'
- iyy='0.001' iyz='0.0'
- izz='0.001'/>
- </inertial>
-
- <collision name="collision">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="100000.0" mu2="100000.0" slip1="100000.0" slip2="100000.0"/>
- </friction>
- </surface>
- </collision>
-
- <visual name="visual">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <material script="Gazebo/FlatBlack"/>
- </visual>
- </link>
-
- <link name="left_wheel">
- <origin pose="0.1 .17 0.11 0 1.5707 1.5707"/>
-
- <inertial mass="0.5">
- <inertia ixx='0.001' ixy='0.0' ixz='0'
- iyy='0.001' iyz='0.0'
- izz='0.001'/>
- </inertial>
-
- <collision name="collision">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="100000.0" mu2="100000.0" slip1="100000.0" slip2="100000.0"/>
- </friction>
- </surface>
- </collision>
-
- <visual name="visual">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <material script="Gazebo/FlatBlack"/>
- </visual>
- </link>
-
- <joint type="revolute" name="laser_joint">
- <origin pose="0 0 0 0 0 0"/>
- <child link="laser"/>
- <parent link="chassis"/>
- <axis xyz="0 0 0">
- <limit lower="0" upper="0"/>
- </axis>
- </joint>
-
- <joint type="revolute" name="left_wheel_hinge">
- <origin pose="0 0 -0.03 0 0 0"/>
- <child link="left_wheel"/>
- <parent link="chassis"/>
- <axis xyz="0 1 0"/>
- </joint>
-
- <joint type="revolute" name="right_wheel_hinge">
- <origin pose="0 0 0.03 0 0 0"/>
- <child link="right_wheel"/>
- <parent link="chassis"/>
- <axis xyz="0 1 0"/>
- </joint>
-
- <joint type="revolute" name="pan_joint_right">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_column_body_right"/>
- <parent link="chassis"/>
- <axis xyz="0 0 1">
- <limit lower='-1.57' upper='1.57'/>
- </axis>
- </joint>
-
- <joint type="revolute" name="tilt_joint_right">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_top_body_right"/>
- <parent link="camera_column_body_right"/>
- <axis xyz="0 1 0">
- <limit lower='-0.4' upper='0.4'/>
- </axis>
- </joint>
-
- <joint type="revolute" name="pan_joint_left">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_column_body_left"/>
- <parent link="chassis"/>
- <axis xyz="0 0 1">
- <limit lower='-1.57' upper='1.57'/>
- </axis>
- </joint>
-
- <joint type="revolute" name="tilt_joint_left">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_top_body_left"/>
- <parent link="camera_column_body_left"/>
- <axis xyz="0 1 0">
- <limit lower='-0.4' upper='0.4'/>
- </axis>
- </joint>
-
-
- <plugin filename="libmotors.so" name="motors">
- <left_joint>left_wheel_hinge</left_joint>
- <right_joint>right_wheel_hinge</right_joint>
- <torque>5</torque>
- </plugin>
-
- <plugin filename="libpose3dencoders.so" name="pose3dencoders">
- <left_joint_pose3dencoders_pan>pan_joint_right</left_joint_pose3dencoders_pan>
- <left_joint_pose3dencoders_tilt>tilt_joint_right</left_joint_pose3dencoders_tilt>
- <right_joint_pose3dencoders_pan>pan_joint_left</right_joint_pose3dencoders_pan>
- <right_joint_pose3dencoders_tilt>tilt_joint_left</right_joint_pose3dencoders_tilt>
- <torque>5</torque>
- </plugin>
-
- <plugin filename="libencoders.so" name="encoders"/>
-
- </model>
-</gazebo>
Deleted: trunk/src/components/gazeboserver/models/pioneer2dxJde.model
===================================================================
--- trunk/src/components/gazeboserver/models/pioneer2dxJde.model 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/models/pioneer2dxJde.model 2013-03-22 11:07:30 UTC (rev 890)
@@ -1,300 +0,0 @@
-<gazebo version='1.0'>
- <model name="pioneer2dx" static="0">
- <link name="chassis">
- <origin pose="0.0 0.0 0.16 0 0 0"/>
- <inertial mass="10.0">
- <inertia ixx='0.01' ixy='0' ixz='0' iyy='0.01' iyz='0' izz='0.01'/>
- </inertial>
- <collision name="collision">
- <geometry>
- <box size="0.445 0.277 0.17"/>
- </geometry>
- </collision>
- <visual name="visual">
- <origin pose="0 0 0.04 0 0 0"/>
- <geometry>
- <mesh filename="pioneer2dx/chassis.dae"/>
- </geometry>
- </visual>
- <collision name="castor_collision">
- <origin pose="-0.200 0 -0.12 0 0 0"/>
- <geometry>
- <sphere radius="0.04"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="2" mu2="2" slip1="1.0" slip2="1.0"/>
- </friction>
- </surface>
- </collision>
- <visual name="castor_visual">
- <origin pose="-0.200 0 -0.12 0 0 0"/>
- <geometry>
- <sphere radius="0.04"/>
- </geometry>
- <material script="Gazebo/FlatBlack"/>
- </visual>
- </link>
-
-
- <link name="right_wheel">
- <origin pose="0.1 -.17 0.11 0 1.5707 1.5707"/>
- <inertial mass="0.5">
- <inertia ixx='0.001' ixy='0.0' ixz='0'
- iyy='0.001' iyz='0.0'
- izz='0.001'/>
- </inertial>
- <collision name="collision">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="100000.0" mu2="100000.0" slip1="0.0" slip2="0.0"/>
- </friction>
- </surface>
- </collision>
- <visual name="visual">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <material script="Gazebo/FlatBlack"/>
- </visual>
- </link>
-
-
- <link name="left_wheel">
- <origin pose="0.1 .17 0.11 0 1.5707 1.5707"/>
-
- <inertial mass="0.5">
- <inertia ixx='0.001' ixy='0.0' ixz='0'
- iyy='0.001' iyz='0.0'
- izz='0.001'/>
- </inertial>
- <collision name="collision">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="100000.0" mu2="100000.0" slip1="0.0" slip2="0.0"/>
- </friction>
- </surface>
- </collision>
- <visual name="visual">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <material script="Gazebo/FlatBlack"/>
- </visual>
- </link>
-
-
- <link name="laser">
- <inertial mass="0.5">
- </inertial>
- <visual name="visual">
- <geometry>
- <mesh filename="hokuyo/hokuyo.dae"/>
- </geometry>
- </visual>
- <origin pose='0.16 0 0.3 0 0 -1.57'/>
- <sensor name='laser' type='ray' always_on='1' update_rate='30' visualize='true'>
- <ray>
- <scan>
- <horizontal samples='180' resolution='1' min_angle='3.14' max_angle='0'/>
- </scan>
- <range min='0.080000000000000002' max='8' resolution='0.01'/>
- </ray>
- <plugin name='laser' filename='liblaser.so'/>
- </sensor>
- </link>
-
- <link name="camera_column_body_left">
- <origin pose="0.12 0.1 0.31 1.57 3.15 0"/>
- <inertial mass="0.5">
-
- </inertial>
-
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_mid.dae" scale="0.0125 0.0275 0.0275"/>
- </geometry>
-
- <material script='Gazebo/Blue'/>
- </visual>
-
- <collision name="castor_collision">
- <geometry>
- <box size="0.1 0.1 0.1"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="10000" mu2="10000" slip1="0.0" slip2="0.0"/>
- </friction>
- </surface>
- </collision>
-
- </link>
-
-
- <link name="camera_column_body_right">
- <origin pose="0.12 -0.1 0.31 1.57 3.15 0"/>
- <inertial mass="0.5">
- <inertia ixx='0.001' ixy='0.0' ixz='0'
- iyy='0.001' iyz='0.0'
- izz='0.001'/>
- </inertial>
-
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_mid.dae" scale="0.0125 0.0275 0.0275"/>
- </geometry>
-
- <material script='Gazebo/Blue'/>
- </visual>
-
- <collision name="castor_collision">
- <geometry>
- <box size="0.1 0.1 0.1"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="10000" mu2="10000" slip1="0.0" slip2="0.0"/>
- </friction>
- </surface>
- </collision>
-
- </link>
-
- <link name="camera_top_body_right">
- <origin pose="0.1 -0.1 0.31 1.57 3.15 0"/>
- <inertial mass="1.0">
- </inertial>
-
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_top.dae" scale="0.04 0.04 0.03"/>
- </geometry>
-
- <material script='Gazebo/Black'/>
- </visual>
-
- <sensor name='cam_sensor_right' type='camera' always_on='1' update_rate='20'>
- <origin pose="0 0 0 1.57 3.15 0"/>
- <camera>
- <horizontal_fov angle='1.57'/>
- <image width='640' height='480' format='R8G8B8'/>
- <clip near='0.5' far='5'/>
- </camera>
- <plugin name='camera_dump' filename='libcamera_dump.so'/>
- </sensor>
- </link>
-
- <link name="camera_top_body_left">
- <origin pose="0.1 0.1 0.31 1.57 3.15 0"/>
- <inertial mass="1.0">
- </inertial>
-
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_top.dae" scale="0.04 0.04 0.03"/>
- </geometry>
-
- <material script='Gazebo/Black'/>
- </visual>
-
- <sensor name='cam_sensor_left' type='camera' always_on='1' update_rate='20'>
- <origin pose="0 0 0 1.57 3.15 0"/>
- <camera>
- <horizontal_fov angle='1.57'/>
- <image width='640' height='480' format='R8G8B8'/>
- <clip near='0.5' far='5'/>
- </camera>
- <plugin name='camera_dump' filename='libcamera_dump.so'/>
- </sensor>
- </link>
-
- <joint type="revolute" name="left_wheel_hinge">
- <origin pose="0 0 -0.03 0 0 0"/>
- <child link="left_wheel"/>
- <parent link="chassis"/>
- <axis xyz="0 1 0"/>
- </joint>
-
- <joint type="revolute" name="right_wheel_hinge">
- <origin pose="0 0 0.03 0 0 0"/>
- <child link="right_wheel"/>
- <parent link="chassis"/>
- <axis xyz="0 1 0"/>
- </joint>
-
- <joint type="revolute" name="laser_joint">
- <origin pose="0.2 0 0.2 0 0 0"/>
- <child link="laser"/>
- <parent link="chassis"/>
- <axis xyz="0 0 0">
- <limit lower="0" upper="0"/>
- </axis>
- </joint>
-
- <joint type="revolute" name="pan_joint_right">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_column_body_right"/>
- <parent link="chassis"/>
- <axis xyz="0 0 1">
- <limit lower='-1.57' upper='1.57'/>
- </axis>
- </joint>
-
- <joint type="revolute" name="tilt_joint_right">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_top_body_right"/>
- <parent link="camera_column_body_right"/>
- <axis xyz="0 1 0">
- <limit lower='-0.4' upper='0.7'/>
- </axis>
- </joint>
-
- <joint type="revolute" name="pan_joint_left">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_column_body_left"/>
- <parent link="chassis"/>
- <axis xyz="0 0 1">
- <limit lower='-1.57' upper='1.57'/>
- </axis>
- </joint>
-
- <joint type="revolute" name="tilt_joint_left">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_top_body_left"/>
- <parent link="camera_column_body_left"/>
- <axis xyz="0 1 0">
- <limit lower='-0.4' upper='0.7'/>
- </axis>
- </joint>
-
- <plugin filename="libmotors.so" name="motors">
- <left_joint>left_wheel_hinge</left_joint>
- <right_joint>right_wheel_hinge</right_joint>
- <torque>5</torque>
- </plugin>
-
- <plugin filename="libpose3dencoders.so" name="pose3dencoders">
- <left_joint_pose3dencoders_pan>pan_joint_right</left_joint_pose3dencoders_pan>
- <left_joint_pose3dencoders_tilt>tilt_joint_right</left_joint_pose3dencoders_tilt>
- <right_joint_pose3dencoders_pan>pan_joint_left</right_joint_pose3dencoders_pan>
- <right_joint_pose3dencoders_tilt>tilt_joint_left</right_joint_pose3dencoders_tilt>
- <torque>5</torque>
- </plugin>
-<!--
- <plugin filename="libpose3dmotors.so" name="pose3dmotors">
- <left_joint_pose3dmotors>camera_joint_left</left_joint_pose3dmotors>
- <right_joint_pose3dmotors>camera_joint_right</right_joint_pose3dmotors>
- <torque>5</torque>
- </plugin>
--->
- <plugin filename="libencoders.so" name="encoders"/>
-
- </model>
-</gazebo>
Deleted: trunk/src/components/gazeboserver/models/pioneer2dxJde.world
===================================================================
--- trunk/src/components/gazeboserver/models/pioneer2dxJde.world 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/models/pioneer2dxJde.world 2013-03-22 11:07:30 UTC (rev 890)
@@ -1,13 +0,0 @@
-<?xml version="1.0"?>
-<gazebo version="1.0">
- <world name="default">
- <!-- Ground -->
- <include filename="ground_plane.model"/>
-
- <!-- Pioneer2dx model -->
- <include filename="pioneer2dxJde.model"/>
-
- <!-- A global light source -->
- <include filename="sun.light"/>
- </world>
-</gazebo>
Deleted: trunk/src/components/gazeboserver/models/pioneer2dxJdeFinal.model
===================================================================
--- trunk/src/components/gazeboserver/models/pioneer2dxJdeFinal.model 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/models/pioneer2dxJdeFinal.model 2013-03-22 11:07:30 UTC (rev 890)
@@ -1,300 +0,0 @@
-<gazebo version='1.0'>
- <model name="pioneer2dx" static="0">
- <link name="chassis">
- <origin pose="0.0 0.0 0.16 0 0 0"/>
- <inertial mass="5.0">
- <inertia ixx='0.01' ixy='0' ixz='0' iyy='0.01' iyz='0' izz='0.01'/>
- </inertial>
- <collision name="collision">
- <geometry>
- <box size="0.445 0.277 0.17"/>
- </geometry>
- </collision>
- <visual name="visual">
- <origin pose="0 0 0.04 0 0 0"/>
- <geometry>
- <mesh filename="pioneer2dx/chassis.dae"/>
- </geometry>
- </visual>
- <collision name="castor_collision">
- <origin pose="-0.200 0 -0.12 0 0 0"/>
- <geometry>
- <sphere radius="0.04"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="0" mu2="0" slip1="1.0" slip2="1.0"/>
- </friction>
- </surface>
- </collision>
- <visual name="castor_visual">
- <origin pose="-0.200 0 -0.12 0 0 0"/>
- <geometry>
- <sphere radius="0.04"/>
- </geometry>
- <material script="Gazebo/FlatBlack"/>
- </visual>
- </link>
-
-
- <link name="right_wheel">
- <origin pose="0.1 -.17 0.11 0 1.5707 1.5707"/>
- <inertial mass="0.5">
- <inertia ixx='0.001' ixy='0.0' ixz='0'
- iyy='0.001' iyz='0.0'
- izz='0.001'/>
- </inertial>
- <collision name="collision">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="100000.0" mu2="100000.0" slip1="0.0" slip2="0.0"/>
- </friction>
- </surface>
- </collision>
- <visual name="visual">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <material script="Gazebo/FlatBlack"/>
- </visual>
- </link>
-
-
- <link name="left_wheel">
- <origin pose="0.1 .17 0.11 0 1.5707 1.5707"/>
-
- <inertial mass="0.5">
- <inertia ixx='0.001' ixy='0.0' ixz='0'
- iyy='0.001' iyz='0.0'
- izz='0.001'/>
- </inertial>
- <collision name="collision">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="100000.0" mu2="100000.0" slip1="0.0" slip2="0.0"/>
- </friction>
- </surface>
- </collision>
- <visual name="visual">
- <geometry>
- <cylinder radius="0.11" length="0.05"/>
- </geometry>
- <material script="Gazebo/FlatBlack"/>
- </visual>
- </link>
-
-
- <link name="laser">
- <inertial mass="0.5">
- </inertial>
- <visual name="visual">
- <geometry>
- <mesh filename="hokuyo/hokuyo.dae"/>
- </geometry>
- </visual>
- <origin pose='0.16 0 0.3 0 0 -1.57'/>
- <sensor name='laser' type='ray' always_on='1' update_rate='30' visualize='true'>
- <ray>
- <scan>
- <horizontal samples='180' resolution='1' min_angle='3.14' max_angle='0'/>
- </scan>
- <range min='0.080000000000000002' max='8' resolution='0.01'/>
- </ray>
- <plugin name='laser' filename='liblaser.so'/>
- </sensor>
- </link>
-
- <link name="camera_column_body_left">
- <origin pose="0.12 0.1 0.31 1.57 3.15 0"/>
- <inertial mass="0.5">
-
- </inertial>
-
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_mid.dae" scale="0.0125 0.0275 0.0275"/>
- </geometry>
-
- <material script='Gazebo/Blue'/>
- </visual>
-
- <collision name="castor_collision">
- <geometry>
- <box size="0.1 0.1 0.1"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="10000" mu2="10000" slip1="0.0" slip2="0.0"/>
- </friction>
- </surface>
- </collision>
-
- </link>
-
-
- <link name="camera_column_body_right">
- <origin pose="0.12 -0.1 0.31 1.57 3.15 0"/>
- <inertial mass="0.5">
- <inertia ixx='0.001' ixy='0.0' ixz='0'
- iyy='0.001' iyz='0.0'
- izz='0.001'/>
- </inertial>
-
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_mid.dae" scale="0.0125 0.0275 0.0275"/>
- </geometry>
-
- <material script='Gazebo/Blue'/>
- </visual>
-
- <collision name="castor_collision">
- <geometry>
- <box size="0.1 0.1 0.1"/>
- </geometry>
- <surface>
- <friction>
- <ode mu="10000" mu2="10000" slip1="0.0" slip2="0.0"/>
- </friction>
- </surface>
- </collision>
-
- </link>
-
- <link name="camera_top_body_right">
- <origin pose="0.1 -0.1 0.31 1.57 3.15 0"/>
- <inertial mass="1.0">
- </inertial>
-
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_top.dae" scale="0.04 0.04 0.03"/>
- </geometry>
-
- <material script='Gazebo/Black'/>
- </visual>
-
- <sensor name='cam_sensor_right' type='camera' always_on='1' update_rate='20'>
- <origin pose="0 0 0 1.57 3.15 0"/>
- <camera>
- <horizontal_fov angle='1.57'/>
- <image width='640' height='480' format='R8G8B8'/>
- <clip near='0.5' far='5'/>
- </camera>
- <plugin name='camera_dump' filename='libcamera_dump.so'/>
- </sensor>
- </link>
-
- <link name="camera_top_body_left">
- <origin pose="0.1 0.1 0.31 1.57 3.15 0"/>
- <inertial mass="1.0">
- </inertial>
-
- <visual name="visual">
- <geometry>
- <mesh filename="sonyvid30/sonyvid30_top.dae" scale="0.04 0.04 0.03"/>
- </geometry>
-
- <material script='Gazebo/Black'/>
- </visual>
-
- <sensor name='cam_sensor_left' type='camera' always_on='1' update_rate='20'>
- <origin pose="0 0 0 1.57 3.15 0"/>
- <camera>
- <horizontal_fov angle='1.57'/>
- <image width='640' height='480' format='R8G8B8'/>
- <clip near='0.5' far='5'/>
- </camera>
- <plugin name='camera_dump' filename='libcamera_dump.so'/>
- </sensor>
- </link>
-
- <joint type="revolute" name="left_wheel_hinge">
- <origin pose="0 0 -0.03 0 0 0"/>
- <child link="left_wheel"/>
- <parent link="chassis"/>
- <axis xyz="0 1 0"/>
- </joint>
-
- <joint type="revolute" name="right_wheel_hinge">
- <origin pose="0 0 0.03 0 0 0"/>
- <child link="right_wheel"/>
- <parent link="chassis"/>
- <axis xyz="0 1 0"/>
- </joint>
-
- <joint type="revolute" name="laser_joint">
- <origin pose="0.2 0 0.2 0 0 0"/>
- <child link="laser"/>
- <parent link="chassis"/>
- <axis xyz="0 0 0">
- <limit lower="0" upper="0"/>
- </axis>
- </joint>
-
- <joint type="revolute" name="pan_joint_right">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_column_body_right"/>
- <parent link="chassis"/>
- <axis xyz="0 0 1">
- <limit lower='-1.57' upper='1.57'/>
- </axis>
- </joint>
-
- <joint type="revolute" name="tilt_joint_right">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_top_body_right"/>
- <parent link="camera_column_body_right"/>
- <axis xyz="0 1 0">
- <limit lower='-0.4' upper='0.7'/>
- </axis>
- </joint>
-
- <joint type="revolute" name="pan_joint_left">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_column_body_left"/>
- <parent link="chassis"/>
- <axis xyz="0 0 1">
- <limit lower='-1.57' upper='1.57'/>
- </axis>
- </joint>
-
- <joint type="revolute" name="tilt_joint_left">
- <origin pose="0 0 0 0 0 0"/>
- <child link="camera_top_body_left"/>
- <parent link="camera_column_body_left"/>
- <axis xyz="0 1 0">
- <limit lower='-0.4' upper='0.7'/>
- </axis>
- </joint>
-
- <plugin filename="libmotors.so" name="motors">
- <left_joint>left_wheel_hinge</left_joint>
- <right_joint>right_wheel_hinge</right_joint>
- <torque>5</torque>
- </plugin>
-
- <plugin filename="libpose3dencoders.so" name="pose3dencoders">
- <left_joint_pose3dencoders_pan>pan_joint_right</left_joint_pose3dencoders_pan>
- <left_joint_pose3dencoders_tilt>tilt_joint_right</left_joint_pose3dencoders_tilt>
- <right_joint_pose3dencoders_pan>pan_joint_left</right_joint_pose3dencoders_pan>
- <right_joint_pose3dencoders_tilt>tilt_joint_left</right_joint_pose3dencoders_tilt>
- <torque>5</torque>
- </plugin>
-<!--
- <plugin filename="libpose3dmotors.so" name="pose3dmotors">
- <left_joint_pose3dmotors>camera_joint_left</left_joint_pose3dmotors>
- <right_joint_pose3dmotors>camera_joint_right</right_joint_pose3dmotors>
- <torque>5</torque>
- </plugin>
--->
- <plugin filename="libencoders.so" name="encoders"/>
-
- </model>
-</gazebo>
Modified: trunk/src/components/gazeboserver/motors.cc
===================================================================
--- trunk/src/components/gazeboserver/motors.cc 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/motors.cc 2013-03-22 11:07:30 UTC (rev 890)
@@ -55,10 +55,16 @@
gzerr << "Unable to find right joint["
<< _sdf->GetElement("right_joint")->GetValueString() << "]\n";
+
+
+ // Listen to the update event. This event is broadcast every
+ // simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateStart(
boost::bind(&Motors::OnUpdate, this));
}
+ // Called by the world update start event
+
void Motors::Init() {
this->wheelSeparation = this->leftJoint->GetAnchor(0).Distance(this->rightJoint->GetAnchor(0));
std::cout << "Wheel Separation:" << this->wheelSeparation << std::endl;
@@ -70,16 +76,33 @@
std::cout << "Wheel Diameter:" << this->wheelRadius * 2 << std::endl;
}
+ /*
+ void Motors::OnVelMsg(ConstPosePtr& _msg) {
+
+ double vr, va;
+
+ vr = _msg->position().x() + 5;
+ std::cout << "vr:" << vr << std::endl;
+ va = msgs::Convert(_msg->orientation()).GetAsEuler().z + 3;
+
+ this->wheelSpeed[LEFT] = vr + va * this->wheelSeparation / 2.0;
+ this->wheelSpeed[RIGHT] = vr - va * this->wheelSeparation / 2.0;
+
+
+
+ }
+ */
+
void Motors::OnUpdate() {
- gettimeofday(&a, NULL);
- totala = a.tv_sec * 1000000 + a.tv_usec;
- cycle = 50;
+ if (count == 0) {
- if (count == 0) {
+ robotMotors.v = 0;
+ robotMotors.w = 0;
+
count++;
std::string name = this->model->GetName();
- std::cout << "GetName() " << name << std::endl;
+ std::cout << "motors name " << name << std::endl;
nameMotors = std::string("--Ice.Config=" + name +"Motors.cfg");
pthread_t thr_gui;
pthread_create(&thr_gui, NULL, &motorsICE, (void*) this);
@@ -88,39 +111,29 @@
double vr, va; //vr -> velocidad lineal; va -> velocidad angular
pthread_mutex_lock(&mutex);
- vr = robotMotors.v; //
- va = robotMotors.w; //
+
+ vr = robotMotors.v/100;
+
+ va = robotMotors.w/10;
+
pthread_mutex_unlock(&mutex);
-
-
+
this->wheelSpeed[LEFT] = vr + va * this->wheelSeparation / 2.0;
this->wheelSpeed[RIGHT] = vr - va * this->wheelSeparation / 2.0;
double leftVelDesired = (this->wheelSpeed[LEFT] / this->wheelRadius);
double rightVelDesired = (this->wheelSpeed[RIGHT] / this->wheelRadius);
+/*
+ std::cout << "leftVelDesired " << leftVelDesired << std::endl;
+ std::cout << "rightVelDesired " << rightVelDesired << std::endl;
+ std::cout << "torque " << torque << std::endl;
+*/
+ this->leftJoint->SetVelocity(0, leftVelDesired);
+ this->rightJoint->SetVelocity(0, rightVelDesired);
- this->leftJoint->SetVelocity(0, leftVelDesired/2);
- this->rightJoint->SetVelocity(0, rightVelDesired/2);
-
-
- this->leftJoint->SetMaxForce(0, 1.4); //this->leftJoint->SetMaxForce(0, this->torque);
- this->rightJoint->SetMaxForce(0, 1.4); //this->rightJoint->SetMaxForce(0, this->torque);
- //this->leftJoint->SetForce(0,1.5);
- // this->rightJoint->SetForce(0,1.5);
-
- gettimeofday(&b, NULL);
- totalb = b.tv_sec * 1000000 + b.tv_usec;
-
- diff = (totalb - totala) / 1000;
- diff = cycle - diff;
-
- if (diff < 10)
- diff = 10;
-
- //usleep(diff*1000);
- sleep(diff/1000);
-
+ this->leftJoint->SetMaxForce(0, this->torque);
+ this->rightJoint->SetMaxForce(0, this->torque);
}
class MotorsI : virtual public jderobot::Motors {
@@ -136,17 +149,19 @@
virtual float getV(const Ice::Current&) {
float v_return;
- pthread_mutex_lock(&pose->mutex);
+ pthread_mutex_lock(&pose->mutexMotor);
+ //v_return = pose->vel;
v_return = pose->robotMotors.v;
- pthread_mutex_unlock(&pose->mutex);
+ pthread_mutex_unlock(&pose->mutexMotor);
return v_return;
};
virtual float getW(const Ice::Current&) {
float w_return;
- pthread_mutex_lock(&pose->mutex);
+ pthread_mutex_lock(&pose->mutexMotor);
+ //w_return = pose->w;
w_return = pose->robotMotors.w;
- pthread_mutex_unlock(&pose->mutex);
+ pthread_mutex_unlock(&pose->mutexMotor);
return w_return;
};
@@ -155,16 +170,18 @@
};
virtual Ice::Int setV(Ice::Float v, const Ice::Current&) {
- pthread_mutex_lock(&pose->mutex);
- pose->robotMotors.v = v/1000;
- pthread_mutex_unlock(&pose->mutex);
+ pthread_mutex_lock(&pose->mutexMotor);
+ //pose->vel = v;
+ pose->robotMotors.v = v;
+ pthread_mutex_unlock(&pose->mutexMotor);
return 0;
};
virtual Ice::Int setW(Ice::Float _w, const Ice::Current&) {
- pthread_mutex_lock(&pose->mutex);
- pose->robotMotors.w = -_w/10;
- pthread_mutex_unlock(&pose->mutex);
+ pthread_mutex_lock(&pose->mutexMotor);
+ //pose->w = _w;
+ pose->robotMotors.w = -_w;
+ pthread_mutex_unlock(&pose->mutexMotor);
return 0;
};
Modified: trunk/src/components/gazeboserver/motors.h
===================================================================
--- trunk/src/components/gazeboserver/motors.h 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/motors.h 2013-03-22 11:07:30 UTC (rev 890)
@@ -60,9 +60,6 @@
double sum;
int count;
- int cycle;
- long totalb, totala, diff;
- struct timeval a, b;
//event::ConnectionPtr updateConnection; Alex
//physics::ModelPtr model; Alex
Modified: trunk/src/components/gazeboserver/pioneer2dxJde.world
===================================================================
--- trunk/src/components/gazeboserver/pioneer2dxJde.world 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/pioneer2dxJde.world 2013-03-22 11:07:30 UTC (rev 890)
@@ -9,12 +9,24 @@
<!-- Pioneer2dx model -->
<include>
<uri>model://pioneer2dxJde</uri>
+ <name>pioneer2dx</name>
</include>
-
+
+ <!-- Pioneer2dx model -->
+ <include>
+ <uri>model://pioneer2dxJde</uri>
+ <name>pioneer2dx2</name>
+ <pose>2 0 0 0 0 0</pose>
+ </include>
+
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
+
+ <include>
+ <uri>model://simpleWorld</uri>
+ </include>
</world>
</sdf>
Deleted: trunk/src/components/gazeboserver/pioneer2dxJde_antiguo.world
===================================================================
--- trunk/src/components/gazeboserver/pioneer2dxJde_antiguo.world 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/pioneer2dxJde_antiguo.world 2013-03-22 11:07:30 UTC (rev 890)
@@ -1,13 +0,0 @@
-<?xml version="1.0"?>
-<gazebo version="1.0">
- <world name="default">
- <!-- Ground -->
- <include filename="ground_plane.model"/>
-
- <!-- Pioneer2dx model -->
- <include filename="pioneer2dx.model"/>
-
- <!-- A global light source -->
- <include filename="sun.light"/>
- </world>
-</gazebo>
Modified: trunk/src/components/gazeboserver/pose3dencoders.cc
===================================================================
--- trunk/src/components/gazeboserver/pose3dencoders.cc 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/pose3dencoders.cc 2013-03-22 11:07:30 UTC (rev 890)
@@ -1,7 +1,6 @@
#include "pose3dencoders.h"
//#include "pose3dmotors.h"
-#define RADTODEG 57.29582790
namespace gazebo {
@@ -14,18 +13,6 @@
pthread_mutex_init(&mutex, NULL);
pthread_mutex_init(&mutexPose3DEncoders, NULL);
count = 0;
- //Init right joint properties defined in models/pioneer2dx.model
- this->cameraRight.motor.maxPan = 1.57;
- this->cameraRight.motor.minPan = -1.57;
- this->cameraRight.motor.maxTilt = 0.4;
- this->cameraRight.motor.minTilt = -0.4;
- //Init left joint properties defined in models/pioneer2dx.model
- this->cameraLeft.motor.maxPan = 1.57;
- this->cameraLeft.motor.minPan = -1.57;
- this->cameraLeft.motor.maxTilt = 0.4;
- this->cameraLeft.motor.minTilt = -0.4;
-
-
std::cout << "constructor pose3dencoders" << std::endl;
}
@@ -42,8 +29,6 @@
this->cameraLeft.joint_pose3dencoders_pan = this->model->GetJoint("pan_joint_left");
this->cameraLeft.joint_pose3dencoders_tilt = this->model->GetJoint("tilt_joint_left");
-
-
if (!this->cameraLeft.joint_pose3dencoders_pan)
gzerr << "Unable to find left joint pose3dencoders_pan["
<< _sdf->GetElement("left_joint_pose3dencoders_pan")->GetValueString() << "]\n";
@@ -98,123 +83,135 @@
void Pose3DEncoders::OnUpdate() {
- gettimeofday(&a, NULL);
- totala = a.tv_sec * 1000000 + a.tv_usec;
- cycle = 50;
-
if (count == 0) {
count++;
- namePose3DEncoders = std::string("--Ice.Config=pose3dencoders.cfg");
+
+ std::string name = this->model->GetName();
+
+ namePose3DEncoders = std::string("--Ice.Config=" + name + "_pose3dencoders.cfg");
pthread_t thr_gui;
pthread_create(&thr_gui, NULL, &pose3dencodersICE, (void*) this);
}
// ----------ENCODERS----------
//GET pose3dencoders data from left_camera (PAN&TILT)
-
- if (this->cameraLeft.camera_link_pan->GetRelativePose().rot.GetAsEuler().z < 0) {
- this->cameraLeft.encoder.pan = -(3.14146 + this->cameraLeft.camera_link_pan->GetRelativePose().rot.GetAsEuler().z);
- } else {
- this->cameraLeft.encoder.pan = 3.14146 - this->cameraLeft.camera_link_pan->GetRelativePose().rot.GetAsEuler().z;
+ this->cameraLeft.encoder.pan = this->cameraLeft.camera_link_pan->GetRelativePose().rot.GetAsEuler().z * 180.0 / M_PI;
+ if (this->cameraLeft.encoder.pan > 0) {
+ this->cameraLeft.encoder.pan = 180 - this->cameraLeft.encoder.pan;
}
+ if (this->cameraLeft.encoder.pan < 0) {
+ this->cameraLeft.encoder.pan = -(180 + this->cameraLeft.encoder.pan);
+ }
- this->cameraLeft.encoder.tilt = this->cameraLeft.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y;
+ //std::cout << this->cameraLeft.encoder.pan << std::endl;
+ this->cameraLeft.encoder.tilt = this->cameraLeft.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y * 180.0 / M_PI;
+ //std::cout << this->cameraLeft.encoder.tilt << std::endl;
+
//GET pose3dencoders data from left_camera (PAN&TILT)
- if (this->cameraRight.camera_link_pan->GetRelativePose().rot.GetAsEuler().z < 0) {
- this->cameraRight.encoder.pan = -(3.14146 + this->cameraRight.camera_link_pan->GetRelativePose().rot.GetAsEuler().z);
- } else {
- this->cameraRight.encoder.pan = 3.14146 - this->cameraRight.camera_link_pan->GetRelativePose().rot.GetAsEuler().z;
+
+ this->cameraRight.encoder.pan = this->cameraRight.camera_link_pan->GetRelativePose().rot.GetAsEuler().z * 180.0 / M_PI;
+ if (this->cameraRight.encoder.pan > 0) {
+ this->cameraRight.encoder.pan = 180 - this->cameraRight.encoder.pan;
}
+ if (this->cameraRight.encoder.pan < 0) {
+ this->cameraRight.encoder.pan = -(180 + this->cameraRight.encoder.pan);
+ }
- this->cameraRight.encoder.tilt = this->cameraRight.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y;
+ //std::cout << this->cameraRight.pan << std::endl;
+ this->cameraRight.encoder.tilt = this->cameraRight.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y * 180.0 / M_PI;
+ //std::cout << this->cameraRight.encoder.tilt << std::endl;
+ double setPanRight = -50;
+ double setPanLeft = -50;
+ double setTiltRight = -10;
+ double setTiltLeft = -10;
// ----------MOTORS----------
+
+
+
if (this->cameraLeft.motor.pan >= 0) {
if (this->cameraLeft.encoder.pan < this->cameraLeft.motor.pan) {
this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, -0.1);
this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+ //std::cout << "AQUI" << std::endl;
} else {
- this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0.1);
+ this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0);
this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
}
} else {
if (this->cameraLeft.encoder.pan > this->cameraLeft.motor.pan) {
this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0.1);
this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+ //std::cout << "AQUI" << std::endl;
} else {
- this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, -0.1);
+ this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0);
this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
- }
+ }
}
if (this->cameraRight.motor.pan >= 0) {
if (this->cameraRight.encoder.pan < this->cameraRight.motor.pan) {
this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, -0.1);
this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+ //std::cout << "AQUI" << std::endl;
} else {
- this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0.1);
+ this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0);
this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
}
} else {
if (this->cameraRight.encoder.pan > this->cameraRight.motor.pan) {
this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0.1);
this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+ //std::cout << "AQUI" << std::endl;
} else {
- this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, -0.1);
+ this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0);
this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
- }
+ }
}
-
-
if (this->cameraLeft.motor.tilt >= 0) {
if (this->cameraLeft.encoder.tilt < this->cameraLeft.motor.tilt) {
this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, -0.1);
this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+ //std::cout << "AQUI" << std::endl;
} else {
- this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0.1);
+ this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0);
this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
}
} else {
- if (this->cameraLeft.encoder.tilt > this->cameraLeft.motor.tilt) {
+ if (this->cameraLeft.encoder.pan > this->cameraLeft.motor.tilt) {
this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0.1);
this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+ //std::cout << "AQUI" << std::endl;
} else {
- this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, -0.1);
+ this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0);
this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
- }
+ }
}
if (this->cameraRight.motor.tilt >= 0) {
if (this->cameraRight.encoder.tilt < this->cameraRight.motor.tilt) {
this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, -0.1);
this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+ //std::cout << "AQUI" << std::endl;
} else {
- this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0.1);
+ this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0);
this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
}
} else {
- if (this->cameraRight.encoder.tilt > this->cameraRight.motor.tilt) {
+ if (this->cameraRight.encoder.pan > this->cameraRight.motor.tilt) {
this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0.1);
this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+ //std::cout << "AQUI" << std::endl;
} else {
- this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, -0.1);
+ this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0);
this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
- }
- }
+ }
+ }
+
- gettimeofday(&b, NULL);
- totalb = b.tv_sec * 1000000 + b.tv_usec;
- diff = (totalb - totala) / 1000;
- diff = cycle - diff;
- if (diff < 10)
- diff = 10;
-
- //usleep(diff*1000);
- sleep(diff / 1000);
-
}
class Pose3DEncodersI : virtual public jderobot::Pose3DEncoders {
@@ -232,6 +229,7 @@
virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current&) {
+ pthread_mutex_lock(&pose->mutex);
pose3DEncodersData->x = 0;
pose3DEncodersData->y = 0;
pose3DEncodersData->z = 0;
@@ -239,10 +237,7 @@
pose3DEncodersData->tilt = pose->cameraLeft.encoder.tilt;
pose3DEncodersData->clock = 0;
pose3DEncodersData->roll = 0;
- pose3DEncodersData->maxPan = pose->cameraLeft.motor.maxPan;
- pose3DEncodersData->minPan = pose->cameraLeft.motor.minPan;
- pose3DEncodersData->maxTilt = pose->cameraLeft.motor.maxTilt;
- pose3DEncodersData->minTilt = pose->cameraLeft.motor.minTilt;
+ pthread_mutex_unlock(&pose->mutex);
return pose3DEncodersData;
@@ -270,6 +265,7 @@
virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current&) {
+ pthread_mutex_lock(&pose->mutex);
pose3DEncodersData->x = 0;
pose3DEncodersData->y = 0;
pose3DEncodersData->z = 0;
@@ -277,10 +273,7 @@
pose3DEncodersData->tilt = pose->cameraRight.encoder.tilt;
pose3DEncodersData->clock = 0;
pose3DEncodersData->roll = 0;
- pose3DEncodersData->maxPan = pose->cameraLeft.motor.maxPan;
- pose3DEncodersData->minPan = pose->cameraLeft.motor.minPan;
- pose3DEncodersData->maxTilt = pose->cameraLeft.motor.maxTilt;
- pose3DEncodersData->minTilt = pose->cameraLeft.motor.minTilt;
+ pthread_mutex_unlock(&pose->mutex);
return pose3DEncodersData;
@@ -292,22 +285,24 @@
jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
};
-
+
class Pose3DMotorsI : virtual public jderobot::Pose3DMotors {
+
public:
-
+
Pose3DMotorsI(gazebo::Pose3DEncoders* pose) : pose3DMotorsData(new jderobot::Pose3DMotorsData()) {
-
+
this->pose = pose;
-
+
}
-
- virtual ~Pose3DMotorsI() {
-
+
+ virtual ~Pose3DMotorsI(){
+
}
-
- virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&) {
-
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&){
+
+ pthread_mutex_lock(&pose->mutex);
pose3DMotorsData->x = 0;
pose3DMotorsData->y = 0;
pose3DMotorsData->z = 0;
@@ -316,25 +311,28 @@
pose3DMotorsData->roll = 0;
pose3DMotorsData->panSpeed = 0;
pose3DMotorsData->tiltSpeed = 0;
+ pthread_mutex_unlock(&pose->mutex);
return pose3DMotorsData;
-
+
+
}
-
+
virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams(const Ice::Current&) {
-
- pose3DMotorsParams->maxPan = pose->cameraLeft.motor.maxPan;
- pose3DMotorsParams->minPan = pose->cameraLeft.motor.minPan;
- pose3DMotorsParams->maxTilt = pose->cameraLeft.motor.maxTilt;
- pose3DMotorsParams->minTilt = pose->cameraLeft.motor.minTilt;
+ pthread_mutex_lock(&pose->mutex);
+ pose3DMotorsParams->maxPan = 0;
+ pose3DMotorsParams->minPan = 0;
+ pose3DMotorsParams->maxTilt = 0;
+ pose3DMotorsParams->minTilt = 0;
pose3DMotorsParams->maxPanSpeed = 0;
pose3DMotorsParams->maxTiltSpeed = 0;
+ pthread_mutex_unlock(&pose->mutex);
return pose3DMotorsParams;
}
-
+
virtual Ice::Int setPose3DMotorsData(const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current&) {
-
+ pthread_mutex_lock(&pose->mutex);
pose->cameraLeft.motor.x = data->x;
pose->cameraLeft.motor.y = data->y;
pose->cameraLeft.motor.z = data->z;
@@ -342,34 +340,37 @@
pose->cameraLeft.motor.tilt = data->tilt;
pose->cameraLeft.motor.roll = data->roll;
pose->cameraLeft.motor.panSpeed = data->panSpeed;
- pose->cameraLeft.motor.tiltSpeed = data->tiltSpeed;
+ pose->cameraLeft.motor.tiltSpeed = data->tiltSpeed;
+ pthread_mutex_unlock(&pose->mutex);
- }
-
+ }
+
gazebo::Pose3DEncoders* pose;
-
+
private:
jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
-
-
+
+
};
class Pose3DMotorsII : virtual public jderobot::Pose3DMotors {
+
public:
-
+
Pose3DMotorsII(gazebo::Pose3DEncoders* pose) : pose3DMotorsData(new jderobot::Pose3DMotorsData()) {
-
+
this->pose = pose;
-
+
}
-
- virtual ~Pose3DMotorsII() {
-
+
+ virtual ~Pose3DMotorsII(){
+
}
-
- virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&) {
-
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&){
+
+ pthread_mutex_lock(&pose->mutex);
pose3DMotorsData->x = 0;
pose3DMotorsData->y = 0;
pose3DMotorsData->z = 0;
@@ -378,25 +379,28 @@
pose3DMotorsData->roll = 0;
pose3DMotorsData->panSpeed = 0;
pose3DMotorsData->tiltSpeed = 0;
+ pthread_mutex_unlock(&pose->mutex);
return pose3DMotorsData;
-
+
+
}
-
+
virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams(const Ice::Current&) {
-
- pose3DMotorsParams->maxPan = pose->cameraRight.motor.maxPan;
- pose3DMotorsParams->minPan = pose->cameraRight.motor.minPan;
- pose3DMotorsParams->maxTilt = pose->cameraRight.motor.maxTilt;
- pose3DMotorsParams->minTilt = pose->cameraRight.motor.minTilt;
+ pthread_mutex_lock(&pose->mutex);
+ pose3DMotorsParams->maxPan = 0;
+ pose3DMotorsParams->minPan = 0;
+ pose3DMotorsParams->maxTilt = 0;
+ pose3DMotorsParams->minTilt = 0;
pose3DMotorsParams->maxPanSpeed = 0;
pose3DMotorsParams->maxTiltSpeed = 0;
+ pthread_mutex_unlock(&pose->mutex);
return pose3DMotorsParams;
}
-
+
virtual Ice::Int setPose3DMotorsData(const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current&) {
-
+ pthread_mutex_lock(&pose->mutex);
pose->cameraRight.motor.x = data->x;
pose->cameraRight.motor.y = data->y;
pose->cameraRight.motor.z = data->z;
@@ -404,18 +408,21 @@
pose->cameraRight.motor.tilt = data->tilt;
pose->cameraRight.motor.roll = data->roll;
pose->cameraRight.motor.panSpeed = data->panSpeed;
- pose->cameraRight.motor.tiltSpeed = data->tiltSpeed;
+ pose->cameraRight.motor.tiltSpeed = data->tiltSpeed;
+ pthread_mutex_unlock(&pose->mutex);
- }
-
+ }
+
gazebo::Pose3DEncoders* pose;
-
+
private:
jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
-
+
+
};
-
+
+
void *pose3dencodersICE(void* v) {
gazebo::Pose3DEncoders* base = (gazebo::Pose3DEncoders*)v;
@@ -429,40 +436,46 @@
ic = Ice::initialize(argc, argv);
+
prop = ic->getProperties();
- std::string Endpoints1 = prop->getProperty("Pose3DEncodersLeft.Endpoints");
- std::cout << "Pose3DEncodersLeft Endpoints > " << Endpoints1 << std::endl;
- std::string Endpoints2 = prop->getProperty("Pose3DEncodersRight.Endpoints");
- std::cout << "Pose3DEncodersRight Endpoints > " << Endpoints2 << std::endl;
- std::string Endpoints3 = prop->getProperty("Pose3DMotorsLeft.Endpoints");
- std::cout << "Pose3DMotorsLeft Endpoints > " << Endpoints3 << std::endl;
- std::string Endpoints4 = prop->getProperty("Pose3DMotorsRight.Endpoints");
- std::cout << "Pose3DMotorsRight Endpoints > " << Endpoints4 << std::endl;
+ std::string Endpoints1 = prop->getProperty("Pose3DEncoders1.Endpoints");
+ std::cout << "Pose3DEncoders1 Endpoints > " << Endpoints1 << std::endl;
+ std::string Endpoints2 = prop->getProperty("Pose3DEncoders2.Endpoints");
+ std::cout << "Pose3DEncoders2 Endpoints > " << Endpoints2 << std::endl;
+ std::string Endpoints3 = prop->getProperty("Pose3DMotors1.Endpoints");
+ std::cout << "Pose3DMotors1 Endpoints > " << Endpoints3 << std::endl;
+ std::string Endpoints4 = prop->getProperty("Pose3DMotors2.Endpoints");
+ std::cout << "Pose3DMotors2 Endpoints > " << Endpoints4 << std::endl;
+
Ice::ObjectAdapterPtr adapter1 =
- ic->createObjectAdapterWithEndpoints("Pose3DEncodersLeft", Endpoints1);
+ ic->createObjectAdapterWithEndpoints("Pose3DEncoders1", Endpoints1);
Ice::ObjectAdapterPtr adapter2 =
- ic->createObjectAdapterWithEndpoints("Pose3DEncodersRight", Endpoints2);
+ ic->createObjectAdapterWithEndpoints("Pose3DEncoders2", Endpoints2);
Ice::ObjectAdapterPtr adapter3 =
- ic->createObjectAdapterWithEndpoints("Pose3DMotorsLeft", Endpoints3);
+ ic->createObjectAdapterWithEndpoints("Pose3DMotors1", Endpoints3);
Ice::ObjectAdapterPtr adapter4 =
- ic->createObjectAdapterWithEndpoints("Pose3DMotorsRight", Endpoints4);
+ ic->createObjectAdapterWithEndpoints("Pose3DMotors2", Endpoints4);
+
+
Ice::ObjectPtr object1 = new Pose3DEncodersI(base);
Ice::ObjectPtr object2 = new Pose3DEncodersII(base);
Ice::ObjectPtr object3 = new Pose3DMotorsI(base);
Ice::ObjectPtr object4 = new Pose3DMotorsII(base);
- adapter1->add(object1, ic->stringToIdentity("Pose3DEncodersLeft"));
- adapter2->add(object2, ic->stringToIdentity("Pose3DEncodersRight"));
- adapter3->add(object3, ic->stringToIdentity("Pose3DMotorsLeft"));
- adapter4->add(object4, ic->stringToIdentity("Pose3DMotorsRight"));
+
+ adapter1->add(object1, ic->stringToIdentity("Pose3DEncoders1"));
+ adapter2->add(object2, ic->stringToIdentity("Pose3DEncoders2"));
+ adapter3->add(object3, ic->stringToIdentity("Pose3DMotors1"));
+ adapter4->add(object4, ic->stringToIdentity("Pose3DMotors2"));
+
adapter1->activate();
adapter2->activate();
adapter3->activate();
adapter4->activate();
-
+
ic->waitForShutdown();
} catch (const Ice::Exception& e) {
std::cerr << e << std::endl;
Modified: trunk/src/components/gazeboserver/pose3dencoders.h
===================================================================
--- trunk/src/components/gazeboserver/pose3dencoders.h 2013-03-20 08:21:35 UTC (rev 889)
+++ trunk/src/components/gazeboserver/pose3dencoders.h 2013-03-22 11:07:30 UTC (rev 890)
@@ -27,8 +27,6 @@
std::string namePose3DEncoders;
pthread_mutex_t mutex;
pthread_mutex_t mutexPose3DEncoders;
-
-
struct pose3dencoders_t {
float x;
@@ -75,9 +73,6 @@
physics::ModelPtr model;
event::ConnectionPtr updateConnection;
int count;
- int cycle;
- long totalb, totala, diff;
- struct timeval a, b;
};
More information about the Jderobot-admin
mailing list