HeeroYui-deprecated/nao_dcm
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Made parameters on parameter server private. Small workaround for DCM stiffness not turning on.

Browse Source
This commit is contained in:
Konstantinos Chatzilygeroudis 2014-05-14 17:42:48 +03:00
parent 07d263ff23
commit ba0588aca2
8 changed files with 206 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp
View File

@ -122,13 +122,14 @@ void NaoCamera::disconnect()
void NaoCamera::loadParams()
{
node_handle_.param("Nao_UseCamera", camera_enabled_, true);
node_handle_.param("Nao_TopCameraEnabled", camera_top_enabled_, camera_enabled_);
node_handle_.param("Nao_BottomCameraEnabled", camera_bottom_enabled_, camera_enabled_);
ros::NodeHandle n_p("~");
n_p.param("UseCamera", camera_enabled_, true);
n_p.param("TopCameraEnabled", camera_top_enabled_, camera_enabled_);
n_p.param("BottomCameraEnabled", camera_bottom_enabled_, camera_enabled_);
node_handle_.param("Nao_TopicQueue", topic_queue_, 50);
n_p.param("TopicQueue", topic_queue_, 50);
node_handle_.param("Nao_Prefix", prefix_, string("nao_dcm"));
n_p.param("Prefix", prefix_, string("nao_dcm"));
prefix_ = prefix_+"/";
}
@ -379,6 +380,7 @@ int main( int argc, char** argv )
string pip = "127.0.0.1";
ros::init(argc, argv, "nao_dcm_camera");
ros::NodeHandle n;
ros::NodeHandle n_p("~");
ros::NodeHandle top("CameraTopNode"), bottom("CameraBottomNode");
if(!ros::master::check())
{
@ -394,11 +396,11 @@ int main( int argc, char** argv )
double communication_rate;
// Load Params from Parameter Server
n.param("Nao_RobotIP", pip, string("127.0.0.1"));
n.param("Nao_RobotPort", pport,9559);
n.param("Nao_CameraBrokerPort", broker_port, 54001);
n.param("Nao_CameraBrokerIP", broker_ip, string("0.0.0.0"));
n.param("Nao_CameraFrequency", communication_rate, 30.0);
n_p.param("RobotIP", pip, string("127.0.0.1"));
n_p.param("RobotPort", pport,9559);
n_p.param("CameraBrokerPort", broker_port, 54001);
n_p.param("CameraBrokerIP", broker_ip, string("0.0.0.0"));
n_p.param("CameraFrequency", communication_rate, 30.0);
// Create your own broker
boost::shared_ptr<AL::ALBroker> broker;

42
nao_dcm_robot/nao_dcm_bringup/config/nao_dcm.yaml
View File

@ -1,36 +1,36 @@
# DO NOT CHANGE IT
Nao_Version: V4
Version: V4
# Choose what components are enabled on startup
Nao_SonarEnabled: true
Nao_TactilesEnabled: true
Nao_BumpersEnabled: true
Nao_FootContactsEnabled: true
SonarEnabled: true
TactilesEnabled: true
BumpersEnabled: true
FootContactsEnabled: true
# Camera related parameters
Nao_UseCamera: true
Nao_TopCameraEnabled: true
Nao_BottomCameraEnabled: true
UseCamera: true
TopCameraEnabled: true
BottomCameraEnabled: true
# Application related parameters
Nao_PublishIMU: true
PublishIMU: true
# ROS related paramaters
Nao_TopicQueue: 10
TopicQueue: 10
# Robot-Connection related parameters
Nao_RobotPort: 9559
Nao_RobotIP: nao.local
Nao_DriverBrokerPort: 54000
Nao_DriverBrokerIP: 0.0.0.0
Nao_CameraBrokerPort: 54001
Nao_CameraBrokerIP: 0.0.0.0
RobotPort: 9559
RobotIP: nao.local
DriverBrokerPort: 54000
DriverBrokerIP: 0.0.0.0
CameraBrokerPort: 54001
CameraBrokerIP: 0.0.0.0
# Communication related parameters
Nao_HighCommunicationFrequency: 50
Nao_LowCommunicationFrequency: 10
Nao_ControllerFrequency: 10
Nao_CameraFrequency: 15
HighCommunicationFrequency: 25
LowCommunicationFrequency: 10
ControllerFrequency: 10
CameraFrequency: 15
# DO NOT CHANGE IT
Nao_Prefix: nao_dcm
Prefix: nao_dcm

12
nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup.launch
View File

@ -3,27 +3,25 @@
<!-- Call Nao Robot publisher -->
<include file="$(find nao_dcm_description)/launch/nao_dcm_H21_urdf.launch" />
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Nao Diagnostics -->
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
</node>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="Nao_BodyType" value="H21"/>
<!-- Call Nao Robot Trajectory Controller -->
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H21.launch"/>
<!-- Run Dashboard -->
<!-- Do Not Run Dashboard -->
<!-- <node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/> -->
<!-- Call Nao Robot Driver -->
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="BodyType" value="H21"/>
</node>
<!-- Call Nao Camera -->

14
nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup_remote.launch
View File

@ -3,17 +3,11 @@
<!-- Call Nao Robot publisher -->
<include file="$(find nao_dcm_description)/launch/nao_dcm_H21_urdf.launch" />
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Nao Diagnostics -->
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
</node>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="Nao_BodyType" value="H21"/>
<!-- Call Nao Robot Trajectory Controller -->
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H21.launch"/>
@ -24,12 +18,20 @@
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="BodyType" value="H21"/>
</node>
<!-- Call Nao Camera -->
<node pkg="nao_dcm_camera" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="BodyType" value="H21"/>
</node>
</launch>

12
nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup.launch
View File

@ -3,27 +3,25 @@
<!-- Call Nao Robot publisher -->
<include file="$(find nao_dcm_description)/launch/nao_dcm_H25_urdf.launch" />
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Nao Diagnostics -->
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
</node>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="Nao_BodyType" value="H25"/>
<!-- Call Nao Robot Trajectory Controller -->
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H25.launch"/>
<!-- Run Dashboard -->
<!-- Do Not Run Dashboard -->
<!-- <node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" args="-s nao_dcm_rqt_dashboard" output="screen"/> -->
<!-- Call Nao Robot Driver -->
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="BodyType" value="H25"/>
</node>
<!-- Call Nao Camera -->

14
nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup_remote.launch
View File

@ -3,17 +3,11 @@
<!-- Call Nao Robot publisher -->
<include file="$(find nao_dcm_description)/launch/nao_dcm_H25_urdf.launch" />
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Nao Diagnostics -->
<node pkg="diagnostic_aggregator" type="aggregator_node" name="nao_dcm_diagnostic_aggregator">
<rosparam file="$(find nao_dcm_driver)/config/nao_dcm_analyzers.yaml" command="load"/>
</node>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="Nao_BodyType" value="H25"/>
<!-- Call Nao Robot Trajectory Controller -->
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H25.launch"/>
@ -24,12 +18,20 @@
<node pkg="nao_dcm_driver" type="nao_dcm_driver" name="nao_dcm_driver" respawn="false" output="screen" >
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
<remap from="/controller_manager" to="/nao_dcm/controller_manager"/>
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="BodyType" value="H25"/>
</node>
<!-- Call Nao Camera -->
<node pkg="nao_dcm_camera" type="nao_dcm_camera" name="nao_dcm_camera" respawn="false" output="screen">
<remap from="CameraTopNode/set_camera_info" to="/nao_dcm/CameraTop/set_camera_info"/>
<remap from="CameraBottomNode/set_camera_info" to="/nao_dcm/CameraBottom/set_camera_info"/>
<!-- Load configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_bringup)/config/nao_dcm.yaml" command="load"/>
<!-- Specify Robot Body Type on Parameter Server -->
<param name="BodyType" value="H25"/>
</node>
</launch>

38
nao_dcm_robot/nao_dcm_driver/src/nao.cpp
View File

@ -265,9 +265,16 @@ bool Nao::initialize()
// Create Joints Hardness Alias
commandAlias[0] = string("JointsHardness");
commandAlias[1].arraySetSize(number_of_joints_-1);
int k = 0;
for(int i=0;i<number_of_joints_;i++)
{
commandAlias[1][i] = string("Device/SubDeviceList/"+joint_names_[i]+"/Hardness/Actuator/Value");
if(joint_names_[i] == "RHipYawPitch")
{
k = -1;
i++;
}
commandAlias[1][i+k] = string("Device/SubDeviceList/"+joint_names_[i]+"/Hardness/Actuator/Value");
}
dcm_proxy_.createAlias(commandAlias);
@ -287,8 +294,8 @@ bool Nao::initialize()
}
// Turn Stiffness On
vector<float> stiff = vector<float>(number_of_joints_,1.0f);
vector<int> times = vector<int>(number_of_joints_,0);
vector<float> stiff = vector<float>(number_of_joints_-1,1.0f);
vector<int> times = vector<int>(number_of_joints_-1,0);
DCMAliasTimedCommand("JointsHardness",stiff, times);
stiffnesses_enabled_ = true;
@ -460,25 +467,26 @@ void Nao::subscribe()
void Nao::loadParams()
{
ros::NodeHandle n_p("~");
// Load Server Parameters
node_handle_.param("Nao_Version", version_, string("V4"));
node_handle_.param("Nao_BodyType", body_type_, string("H21"));
n_p.param("Version", version_, string("V4"));
n_p.param("BodyType", body_type_, string("H21"));
node_handle_.param("Nao_TactilesEnabled", tactiles_enabled_, true);
node_handle_.param("Nao_BumpersEnabled", bumpers_enabled_, true);
node_handle_.param("Nao_SonarEnabled", sonar_enabled_, true);
node_handle_.param("Nao_FootContactsEnabled", foot_contacts_enabled_, true);
n_p.param("TactilesEnabled", tactiles_enabled_, true);
n_p.param("BumpersEnabled", bumpers_enabled_, true);
n_p.param("SonarEnabled", sonar_enabled_, true);
n_p.param("FootContactsEnabled", foot_contacts_enabled_, true);
node_handle_.param("Nao_PublishIMU", imu_published_, true);
n_p.param("PublishIMU", imu_published_, true);
node_handle_.param("Nao_TopicQueue", topic_queue_, 50);
n_p.param("TopicQueue", topic_queue_, 50);
node_handle_.param("Nao_Prefix", prefix_, string("nao_dcm"));
n_p.param("Prefix", prefix_, string("nao_dcm"));
prefix_ = prefix_+"/";
node_handle_.param("Nao_LowCommunicationFrequency", low_freq_, 10.0);
node_handle_.param("Nao_HighCommunicationFrequency", high_freq_, 100.0);
node_handle_.param("Nao_ControllerFrequency", controller_freq_, 15.0);
n_p.param("LowCommunicationFrequency", low_freq_, 10.0);
n_p.param("HighCommunicationFrequency", high_freq_, 100.0);
n_p.param("ControllerFrequency", controller_freq_, 15.0);
}
void Nao::brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier)

13
nao_dcm_robot/nao_dcm_driver/src/nao_driver.cpp
View File

@ -27,6 +27,7 @@ int main( int argc, char** argv )
string pip = "127.0.0.1";
ros::init(argc, argv, "nao_dcm_driver");
ros::NodeHandle n;
ros::NodeHandle n_p("~");
if(!ros::master::check())
{
cerr<<"Could not contact master!\nQuitting... "<<endl;
@ -40,16 +41,20 @@ int main( int argc, char** argv )
string broker_ip = "0.0.0.0";
// Load Params from Parameter Server
n.param("Nao_RobotIP", pip, string("127.0.0.1"));
n.param("Nao_RobotPort", pport,9559);
n.param("Nao_DriverBrokerPort", broker_port, 54000);
n.param("Nao_DriverBrokerIP", broker_ip, string("0.0.0.0"));
n_p.param("RobotIP", pip, string("127.0.0.1"));
n_p.param("RobotPort", pport,9559);
n_p.param("DriverBrokerPort", broker_port, 54000);
n_p.param("DriverBrokerIP", broker_ip, string("0.0.0.0"));
// Create your own broker
boost::shared_ptr<AL::ALBroker> broker;
try
{
broker = AL::ALBroker::createBroker(broker_name,broker_ip,broker_port,pip,pport,0);
// Workaround because stiffness does not work well via DCM
AL::ALProxy tempMotion("ALMotion", pip, pport);
tempMotion.callVoid("setStiffnesses","Body",1.0f);
}
catch(...)
{